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Abstract 


A model of the Orbital Maneuvering Vehicle is 
presented. In this model, several simplifications have been 
made. A set of hand controller signals may be used to 
control the motion of the OMV. 

Model verification is carried out using a sequence of 
tests. The dynamic variables generated by the model is 
compared, whenever possible, with the corresponding 
analytical ones. The result of the tests show conclusively 
that the present model is behaving correctly. Further, 
this model interfaces properly with the State Vector Trans- 
formation module (SVX) developed previously. Correct com- 
mand sequences are generated by the OMV and SVX system, and 
these command sequences can be used to drive the flat floor 
simulation system here at MSFC. 





INTRODUCTION 


This report discusses the design and implementation of OMV 
— a mathematical model of the Orbital Maneuvering Vehicle. To 
avoid confusion, the term OMV shall be used to mean the mathe- 
matical model as well as the software that performs the 
modelling, while the full terra "Orbital Maneuvering Vehicle" 
shall be used to mean the actual flight hardware. 

The Orbital Maneuvering Vehicle can be maneuvered by remote 
operator control. Its motion is completely specified by its 
equations of motion. The solution of the equations of motion 

m • • • rri 

yields its position [X,Y,Z] , velocity [X,Y,Z] , orientation 
[ r , p , y ] T and their rates [r,p,y]^ where r, p and y stand for 
roll, pitch and yaw respectively. From these dynamic quantities, 
a 14-component state vector can be generated. This state vector 
contains all the necessary information to completely specify the 
state of the vehicle in space at any time. 

The OMV simulates the motion of the Orbital Maneuvering 
Vehicle in space. OMV is a software subsystem that is an integral 
part of the software system used to drive the MSFC flat floor 
simulation system. In this installation, a set of hand con- 
trollers is used to maneuver the OMV (Mathematical model) and the 
state vector obtained is used as input to a second software 
module called SVX (the State Vector Transformation module) which 
transforms it to a suitable set of commands to be transmitted to, 
and thereby controlling the mobile base on the flat floor. The 
over-all relation is as shown in Figure 1. As can be seen in this 
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figure, the OMV module encompasses the vehicle response module as 
well as the orbital mechanics module. In order to optimize exe- 
cution speed, these two modules are not implemented as separate 
entities . 

The State Vector Transformation Module has been discussed 
elsewhere (see Reference 1) and will not be elaborated here. 
Throughout this report, it is important to bear in mind that the 
OMV simulates the motion of the Orbital Maneuvering vehicle but 
otherwise has no physical relationship with the Orbital Maneu- 
vering Vehicle. The mobile base on the flat floor will attempt to 
move in such a manner that a mock-up module mounted on it will 
replicate the motion of the Orbital Maneuvering Vehicle, using a 
set of commands derived from the state vectors generated by OMV. 
Otherwise the mobile base is not related to the OMV. The mock-up 
module is not the Orbital Maneuvering Vehicle. One of the objec- 
tives of the flat floor system is to simulate docking of the OMV 
with a target vehicle. 

THE OMV MODEL 

This report describes a simplified mathematical model of the 
Orbital Maneuvering Vehicle. A more detailed model is being 
developed elsewhere at MSFC. In the present model, several sim- 
plifications and assumptions have been made. The objective is to 
develop quickly (and hence the simplification) a model that can 
be used to drive the flat floor system. 

Before discussing the model in any detail, it is necessary 
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to define the various coordinate systems used in this work. 

A. The Local Vertical Frame (LVF) 

Imagine a space craft in an orbit around the earth. It 
is immaterial whether this is the Orbital Maneuvering Vehi- 
cle or the target vehicle. LVF is a coordinate system with 
its origin at the center of mass of this space craft such 
that Z-axis lies in the plane of the orbit and is directed 
away from the center of the earth. The Y-axis is chosen to 
be parallel to the orbital angular momentum vector and X- 
axis is tangential to the orbit as shown in Figure 2. The 
position, velocity as well as orientation of the second 
vehicle are described in LVF and is therefore relative to 
the orbiting vehicle. Throughout this work, we shall assume 
that the target vehicle is the orbiting vehicle. 

B. OMV Body Frame 

This is a body fixed reference frame with its origin 
fixed at the center of mass of the OMV, and its axes will be 
denoted by 1, 2 and 3 respectively. Initially, at the start 

of the simulation, 1, 2 and 3 axes line up with X, Y and Z 
axes respectively. As can be seen from Figure 3, the axis of 
symmetry is the 1-axis. 

In order to construct the model of the Orbital Maneuvering 

Vehicle, the following assumptions are made : 

1. The OMV is assumed to be a circular disk of constant mass 
and having a uniform mass distribution. This assumption may 
seem unreasonable at first glance, but one quickly realizes 
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that the detail shape of the OMV is unimportant as long as 
one knows the mass and propulsion characteristics of the 
Orbital Maneuvering Vehicle. In the present model, the mass 
characteristics are summarized in Table 1. These figures are 
taken from the MSFC Preliminary Definition Studies (see 
Reference 2). 

2. The OMV is manipulated using signals from a set of hand 

controllers. These signal can be classified into two groups. 
The first group is used to simulate a force acting through 
the center of mass of the OMV. In other words, one can, from 
this group of signals, generate an acceleration vector a = 
[a^, a 2 ,a%] in the body frame. The other group of signals 
simulates rotations about 1, 2 and 3 axes, namely, a vector 
w = [ w ^ , W 2 , W 3 . Assumptions 1 and 2 mean that detailed 

knowledge of the shape, thrust level and placement of the 
thruster and so forth are not really needed. The present 
control mode is the only mode implemented. 

3. Circular orbits are assumed. The altitude of the orbit can 
be anything from 150 to 1500 nautical miles which is the 
designed operating range of the Orbital Maneuvering Vehicle. 

4. Orbital mechanics is an important part in describing the 
motion of the OMV and is therefore implemented. Other 
secondary perturbation effects are totally ignored. 

5. The state of the OMV is computed and updated 10 times per 
second. The period of 0.1 second will be referred to as a 
major cycle throughout this report. 
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The equations of motion of the OMV can be discussed in terms 
of the rotational part and translational part. 

Rotational Equations of Motion 

The rotational equation of motion can be written as : 

t = L 


where L = Iw is the angular mementum vector and x is the applied 
torque. I is the moment of inertia tensor and v is the body rate. 
The solution can be drastically simplified by choosing the body 
axes 1, 2 and 3 such that I is diagonal (Please see References 3 
and 4 ) , that is : 


I 


J 11 

0 

0 


0 

1 22 
0 


0 

0 

I 33 


T 

Remember that v = [wj.w 2 .w 3] 1 is obtained from the hand control- 
ler signals. The solution of the rotational equations of motion 
yields <p , 9 and ^ the three Euler angles. The order and sense 
of rotation is chosen in the conventional manner (Please see 
Reference 5), that is : 


[ ] 1 [ © ]3[ V ] 2 

To reduce computational overhead, quaternions are used to specify 
the attitude of the OMV rather than the Euler angles themselves. 
It has been proven that the two representations are exactly 
equivalent (Reference 6). A quaternion q may be written as : 
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q = iq 1 + jq 2 + kq 3 + q^ = [ q ^ , q 2 . ^3 . Qz, ] T 

and satisfies the relation 

2 2 2 2 

Ql + Q2 + Q3 + ^4 = 1 

An object whose attitude is described by the three Euler angles 
relative to some reference frame can be treated, as a single 
rotation by a about an Euler axis E = [Ei,E 2 ,E 3 ] . Theory has 
shown that this is the shortest angular path (Reference 7) in the 
sense that a is less than the algebraic sum of f , 0 and ^ . 
The angle a and the Euler axis can be expressed in terms of the 
quaternion q as : 

a 

cos ~T ~ ^4 

E = (iq l + jq 2 + kq 3 ) / (q x + q 2 + q 3 ) 2 

Since the attitude control system of the OMV can control the 
roll, pitch and yaw axis independently, we expect the roll, pitch 
and yaw [r,p,y]^ to be proportional to the respective components 
of E (Reference 7). In fact, the following relation holds : 

[r,p,y] T = [ aE x , aE y , aE z ] T 

Quaternion algebra leads to further computational economy when 
successive rotations need to be calculated. Let say, at any 
instant, the attitude of the OMV is specified by the quaternion 
q 1 relative to some non-rotating frame . Suppose further that an 
instant later, the vehicle's attitude has changed, having rotated 
by d’ , 9 and 4 1 . These angular displacements are measured rela- 

tive to the rotated body frame. If the new attitude is described 
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by a second quaternion q 2 » the attitude of the vehicle, relative 
to the non-rotatins frame (References 8,9) is then given by 

9 = 9x92 

This is an important advantage because if at the beginning of the 
simulation, the body frame is aligned with the LVF (as specified 
by the quaternion qQ = [0,0, 0,1]^), then the attitude of the OMV 
relative to the LVF, after n successive rotations is simply: 

9 = 9o9i 92 9 n 

Of course, the attitude of the vehicle after the n+l-th rotation 
is q = 9 n 9 n+ i* Thus, the attitude of the vehicle can be computed 
from the previous quaternions. This recursive property gives rise 
to quite a computational advantage, especially since there are 
only four elements in a given quaternion versus the nine elements 
of a direction cosine matrix. 

TRANSLATIONAL EQUATION OF MOTION 

The translational equations of motion has been derived in 
detail in Appendix I, and will not be repeated here. In essence, 
we seek solutions to a set of three simultaneous, coupled second 
order differential equations of the form : 

X = A x - 2 a) Z 

M 2 

Y = A y - . a, Y 

K • 2 

Z = A + 2 a) X + 3 co Z 

u 

m « • * m 

Here, the position and velocity vectors [X^jZ] 1 and [XjY.Z] 1 
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refer to the position and velocity of the OMV relative to the 
target vehicle, as expressed in Local Vertical Frame, to is the 
orbital velocity, and A = [A x ,Ay,A z ]^ is the linear acceleration 
vector in LVF. Remember that the hand controller signals give 
rise to an acceleration vector a = [a^, 82 * 33 ]^ in OMV body frame. 
Thus, one can obtain A from a using the transformation : 

A = C -1 a 

where C - * is the inverse of the direction cosine matrix which can 

T 

be derived from the quaternion q = [ ,a 2 .a^.a^ ] 1 as: 

*- — 

^4 + Ql ~ <12 " ^3 2(q^q2 - 33^4) 2(q^q3 + q2Q4) 

2 C q 1 q 2 + ^ 3 ^ 4 ) ^4 - Ql + ^2 “ q 3 2(q 2 q3 - q^4) 

2 ( q 1 q3 - q2^4) 2(q2^3 + ^ 1 ^ 4 ) ^4 ” ^1 ” q 2 + ^3 

It is obviously impractical to seek an analytical solution 
to the translational equations of motion. Numerical methods must 
be used. In the present work, the Adam-Bashf orth method is used. 
For this purpose, each major cycle is subdivided into N (normally 
10 , but see later section) sub-intervals, each of which will be 
referred to as a minor cycle. It is necessary that the accelera- 
tion vector A be computed for each minor cycle, and stored in an 
acceleration matrix. At the end of N minor cycles, this accelera- 
tion matrix is used to obtain the numerical solution for the 
entire major cycle. A 14-component state vector is then assem- 
bled, and their components are listed below : 


s(i) - 

S(3) 

relative 

position 

vector 

in 

LVF 

S(4) - 

S( 6 ) 

— relative 

velocity 

vector 

in 

LVF 

S(7) - 

S(9) 

— angular 1 

momentum 

vector 

in 

LVF 
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S ( 1 0 ) - S ( 1 3 ) — attitude quaternion 

S ( 1 4 ) — mass in kilograms 

The angular momentum vector in LVF can be deduced as follows. 
Since the body rate w = [wj,W 2 »Wg] i is known, one can calculate 
LB in body frame using the relation (Reference 10): 

L b = I* 

L = C - 1 L b 

where C - ^ is the inverse of the direction cosine matrix. 

The state vector serves as input to the State Vector Trans- 
formation module (SVX). This module has been designed and imple- 
mented (Reference 1) and will not be repeated here. For complete- 
ness, a copy of the updated report is included in Appendix 2. 

System Design and Implementation 

The design and implementation of the present system is best 
discussed in the following sub-sections : 

A) Hand Controllers 

The hand controllers allow the operator to manipulate the 
Orbital Maneuvering Vehicle in terms of translation and attitude. 
In the present system, hand controller signals are used to maneu- 
ver the OMV. The hardware is configured to provide 12 bits of 
information. The first 6 bits pertain to translation, while the 
remaining 6 bits pertain to attitude control. During development, 
the 12 bits are simulated by reading them from a disk file 
(HNDSGL.DAT) as 12 single digit integers. This process is carried 
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out in a subprogram called IINDCTL. In actual implementation, this 
subprogram must be replaced by a suitable device driver. 

The bit assignment is shown in Table 2. It will be noted 
that 1 will be used to denote the "on" state while 0 will be 
used to denote the "off" state. The subroutine HNDCTL contains 
sufficient logic to ensure that when both bits assigned to a 
given axis are on, they will be treated as both off (that is, no 
acceleration along, or rotation about, that axis) to conserve 
fuel usage. The main purpose of this subroutine is to examine the 
12 bits from the hand controllers and return two vectors a and v 
where 

a = [a^,a 2 »ag] 1 and w = [w^,W 2 »W 3 ] 1 

whose meaning have been explained in the previous section. It is 
important to remember that both a and w are expressed in the OMV 
body frame. 

Ideally, the hand controllers signals should be sensed and 
updated every minor cycle. But because of timing considerations 
they will be sensed once every major cycle, and it is explicitly 
assumed that the bit states do not change during the entire major 
cycle. This is not an unreasonable assumption, since one major 
cycle is 0.1 second, which is in the neighbourhood of the average 
reaction time of the human operator. Besides, the OMV does not 
have a fast response because of its large mass and low thrust 
levels . 

The acceleration vector a must be expressed in LVF before it 
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can be used in solving the equations of motion. Tn the OMV soft- 
ware, this is carried out as mentioned previously by : 

a) calculating the inverse of the direction cosine matrix 
C" 1 , 

b) transforming the vector a to A in LVF, and 

c) placing A in an acceleration matrix AA. 

Step a) is carried out by a subroutine called DCSINV while steps 
b) and c) are carried out by subroutines DMUL and STORE in sub- 
routine MOTION. At the end of the N minor cycles, the subroutine 
SOLVE is invoked to obtain solutions to the equations of motion 
numerical 1 y . 

B) Numerical Solutions : 

A three step Adam-Bashf orth method (References 11-14) is 
used to obtain solutions to the equations of motion. This method 
is well known, and will not be elaborated here. Essentially, the 
set of three coupled second differential equations are re-written 
as a set of six simultaneous first order differential equations, 
and the solution computed. The six initial conditions needed for 
the computation are provided by the six components of the rela- 
tive position and velocity vectors. Subroutine SOLVE takes the 
relative displacement and velocity vectors as initial conditions 
of the previous major cycle, and returns the new position and 
velocity vectors. A surboutine called STATE is then invoked to 
assemble the state vector. 

C) Output Section : 

A subroutine called OUTPUT is responsible for conveying 
information to the outside world. In normal operations, no output 
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is generally expected, but during testing, it is necessary to be 
able to monitor the progress of the simulation. At present, one 
can, via the use of flags, control the form and type of output. 
By way of example, one can request OMV to print a time sequence 
of state vectors at 1 second intervals on the printer, or display 
the position and orientation of the mobile base (on the flat 
floor) graphically, or disable all outputs altogether, 

A fairly simple graphics package called PLOT is implemented 
to provide graphics output. This package is developed for the 
initial software checking only; namely to provide the operator 
with some form of visual output. It must be emphasized that this 
package is hardware dependent, and is not competable with the PDP 
11/34 mini-computer. The present graphics package runs on an IBM 
Personal Computer fitted with a TECMAR GRAPHICS MASTER board and 
an IBM monochrome monitor. A resolution of 640 by 352 is used for 
the package, although the system has a potential resolution of 
720 by 700 pixels (Reference 15). PLOT uses escape codes to 
generate the top or side view of the mobile base (including the 
mock up module). A listing of this package, written in FORTRAN 
77, is included in Appendix 3. It is anticipated that this 
package can be modified to run on the Evans and Sutherland color 
graphics terminal driven by a VAX 780. 

The entire OMV module is written in FORTRAN 77, and all 
floating point computations are carried out in double precision. 
The usual structured programming technique is used. Modular de- 
sign is faithfully adhered to, so that subroutines can be easily 
updated or replaced. At times, efficiency may be sacrificed for 
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code clarity, thereby making the code much easier to maintain and 
modify. During the design phase, flexibility is emphasized. 
Model parameters are inputted from disk files. Thus, modifica- 
tions on the flat floor system will not involve any changes to 
the OMV source code. Appendix 4 shows the various data files 
used. Explanations for the various quantities are included as 
part of the record so that one can easily modify the configura- 
tion, initial conditions and so forth without having to refer to 
the source listing. A complete listing of OMV is included in 
Appendix 5, and a hierarchial chart is shown in Figure 4. 

V) Testing and Results 

Initial testing of the OMV software is conducted using an 
IBM Personal Computer with 8087 arithmetic co-processor. The same 
source code without the graphics option has been uploaded to the 
PDP 11/34 at MSFC and executed successfully. 

The nature of the model is such that the major source of 
error would arise from the numerical solutions of the equations 
of motion. Thus, much effort has been spent to ensure that the 
Adam-.Bashf or th method yields accurate results. An error analysis 
of this method shows that the error is of the order of h^ where h 
is the step size. In the present work, the step size is typically 
0.01. This, coupled with the fact that all computations are 
carried out in double precision, means that the expected trunca- 
tion error is of the order of 10 - ^ — a figure that is too good 
to be true. 

The following tests were conducted to verify that this 
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method does indeed give accurate solutions. The homogeneous case 
is first considered. Physically, this corresponds to the situa- 
tion where the operator leaves all the controls in neutral so 
that 

a = [0,0, 0] T and w = [0,0,0]^ 

Thus, the equations of motion reduce to : 

X = - 2 w Z 

2 

Y = - “ Y 

Z = 2“ X + 3u 2 Z 

This set of equations can be solved numerically using the Adam- 
Bashforth method. Further, if X^, X 2 > Xg and V^, V 2 » Vg are the 
initial conditions, it can be shown that the analytical solutions 
are : 

X(t) = X x - ^ ■ 3 - t -~^ 4sin - n - t ^ Vj - 6(nt - si not) X 3 - V 3 

X(t) = - (3 - 4cosot) - 6o(l-cosot) X 3 - 2(sin0t) V 3 

Y(t) = (cosat) X 2 + (Slant) ^ 

Y(t) = -n(sinot) X 2 + (cosot) V 2 

z(t) = ^ - " c ° snt ) V x + (4-3cosot) X 3 + V 3 
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Z(t) = 2(sinnt) + 3n(sinfit) + (cosnt) 


Thus, the numerical solutions can be compared directly with the 
analytical ones. Here, ft is the orbital velocity, and for a 
circular orbit, ft can be calculated : 

ft = G M e / (R 0 + H) 3 

where G is the universal gravitation constant, M g is the mass of 
the earth, R Q is the mean earth radius and H is the altitude. 
Note that at higher orbits, ft approaches 0 and the equations 
of motion approach 

X -> 0 
Y -> 0 
Z -> 0 

and better agreement between numerical and analytical results are 
expected for high altitudes than lower orbits. A computer program 
called ADAM has been developed that would, given a set of initial 
conditions, calculate both the numerical and analytical solutions 
to the equations of motion. The source listing of ADAM is shown 
in Appendix 5. In the present set of tests, an altitude of 200 
kilometers ( ft = 0.00118 rad/sec) is used throughout. This alti- 
tude represents the lowest design orbit of the Orbital Maneu- 
vering Vehicle. Table 3 shows a comparison between the analytical 
and numerical solutions at this altitude, using the initial 
conditions : 
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0, 


0 


Xj = 0, x 2 = x 3 = 

V : = 0.05, V 2 = V 3 = 0 


The result shows that the two solutions agree to better than 3 x 
10 ° in 60 minutes, or about 0.03 mil li-meters. This figure is 
well below the expected accuracy of the flat floor simulation 
system. This suprisingly small error comes from the fact that the 
angular velocity ft is quite small.' When ft = 1.0 is used, (this 
angular frequency does not make sense physically, as it repre- 
sents an orbit well below the earth’s surface, but consititutes a 
valid situation mathematically), the errors propagate quite fast 
as to render the comparison meaningless after 10 minutes. 

A second test was carried out at the same altitude, using 
null initial conditions: 


X 

V 


1 

1 




0 

0 


The hand controller signals were chosen to yield a constant 
acceleration along the X-axis in the LVF, that is a = 
[0.025,0,0]^, and the orientation of the 0MV is chosen to be 
aligned to the LVF at t = 0. The result after 4 seconds of 
simulation is shown in Table 4. A plot of the revelant dynamic 
variables as a function of time is shown in Figure 5. The result 
shows that the model behaves exactly as expected; namely that an 
acceleration along the X-axis gives rise to a Z component, as 
dictated by orbital mechanics. If we ignore the Z contribution 
for the time being, one can estimate the value of X and X using 
Newton's laws (this is not an invalid estimate as the time inter- 
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val is quite short compared with the period of rotation) to be X 
= 0.2 meters, and X = 0.1 meter/sec respectively. These figures 
compare very favourably with the numerical results at t = 4 
seconds. 

A very interesting test was conducted in which the OMV is 
made to execute a pure pitch motion. In this test, it is assumed 
that the OMV is orginally at rest, the initial conditions being : 

X 2 = x 3 = 0 

V 2 = v 3 = 0 
p = y = o 

where r, p, y represent the roll, pitch and yaw respectively. A 
pure pitch motion would correspond to a rotation about the 2 - 
axis. Mathematically, 

r = y =0* and p = w 2 = 0 

When the OMV is executed in this mode, the state vectors are fed 
into the SVX module, with the result that the state vector is 
translated into a sequence of commands CMD. This sequence of 
commands is to be transmitted to the flat floor. Table 5 shows 
the revelent commands for the mobile base. As verified by the 
graphics display, the mock up module mounted on the mobile base 
executes a pure pitch at the same rate as the OMV, while the 
mobile base has to translate along the +X direction. In addition, 
the pivot point is progressively lowered as expected. This test 
shows that the modules OMV and SVX are properly interfaced, and 
that correct results are produced. The command strings as out- 
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putted by the system to the flat floor is shown in Figure 6. 

To further ascertain that the system is functioning proper- 
ly, the hand controller signals corresponding to a translation 
along 1-axis and a yaw is generated. The revelant commands to the 
flat floor system is shown in Table 6. A pictorial representation 
of the mobile base and mock up is as shown in Figure 7. Note that 
the path of the center of mass of the mock up exactly duplicates 
that of the OMV. 

In summary, various tests conducted have shown that the OMV- 
SVX system functions properly. By way of example, a pure yaw 
motion of the OMV demands that the mobile base describes a cir- 
cular path as shown in Figure 8. There is just one area that 
needs further investigation, namely timeing considerations. This 
system must be able to complete all the computation within 0.1 
second — a major cycle. When the system is uploaded to the PDP 
11/34, it was discovered that the computer took more than 0.1 
seconds to complete one major cycle of computation. At this 
juncture, one can take one of the following three corrective 
actions : 

a) Use a faster host computer (VAX 780) 

b) Use single precision computation, or 

c) Increase the step size in the numerical methods. 

Of the three choices, the first method is clearly desirable, but 
until the VAX is installed, one must explore the remaining alter- 
natives. Table 7 shows a time comparison between single and 
double precision arithmetic when the OMV is run until identical 
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parameters on the PDP 11/34 computer. The result shows little 
improvement in execution time. This is not surprising since the 
computer is equipped with hardware floating point capability. The 
only remaining recourse is to increase the step size, thereby 
reducing the number of steps (and hence the number of itera- 
tions). It is discovered that the numerical solution to the 
equations of motion took most of the computation time. Table 8 
shows a similar time test for various steps N and retaining 
double precision arithmetic after the code has been suitably 
optimized. The data show that a step size of h = 0.025 seconds (N 
= 4) satisfies the time requirement. The price to be paid is that 
the error associated with the numerical process may increase. 
Table 9 shows a comparison test for N = 10 and N = 4 using the 
program ADAM. The result suggests that there is an optimum N 
somewhere between 4 and 10 in which the error is a minimum, but 
this question is not pursued any further. The result also shows 
that the error does not increase substantially over the same 
period of 60 minutes whether we use N = 10 or N = 4. Using N = 4, 
the deviation from the analytical solution is still much less 
than the accuracy of the flat floor system. 


Conclusion 

The series of tests conducted, some of which are not re- 
ported here, shows that the simplified mathematical of the Or- 
bital Maneuvering Vehicle is functioning properly, and that it 
interfaces properly with the State Vector Transformation module 
SVX to produce correct sequences of commands to the flat floor. 


19 



By choosing a coarser step in the numerical integration process, 
OM V is able to complete all the necessary computation within a 
major cycle, without compromising on the accuracy. The final acid 
test cannot be conducted until the flat floor hardware is opera- 
tional . 
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Table 1 


OMV Mass Characteristics 


Dynamic Variable 

Value 

unit 

Mass M 

3282.75 

kg 

hi 

7048.37 

y 

kg m 

Z 22 

3713.95 

kg m 2 

I 33 

3713.95 

kg m 2 



Table 2 


Hand Controller Bit Assignments 


bit 


1 

2 

3 

4 

5 

6 


Meaning 

Acceleration along +1 direction 
Acceleration along -1 direction 
Acceleration along +2 direction 
Acceleration along -2 direction 
Acceleration along +3 direction 
Acceleration along -3 direction 


7 

+ roll; 

CCW 

rotation 

about 

1-axis 

8 

- roll; 

CW 

rotation 

about 

1-axis 

9 

+ pitch; 

CCW 

rotation 

about 

2-axis 

10 

- pitch; 

CW 

rotation 

about 

2-axis 

11 

+ yaw; 

CCW 

rotation 

about 

3-axis 

12 

- yaw; 

CW 

rotation 

about 

3-axis 



Table 3 


Comparison Between Analytical and Numerical Solutions 


Time 

in 

Minutes 

X (meters) 

Z (meters) 

Numerical 

Analytical 

Numerical 

Analytical 

0 

0.000000 

0.000000 

0.000000 

0.000000 

5 

13.746736 

13.746736 

5.271240 

5.271240 

10 

20.161917 

20.161917 

20.427114 

20.427114 

15 

12.828963 

12.828962 

43.576117 

43.576178 

20 

-12.952950 

-12.952952 

71.829442 

71.829444 

25 

-59.582227 

-59.582233 

101.660919 

101.660923 

30 

-126.855533 

-126.855544 

129.347660 

129.347664 

35 

-211.993176 

-211.993191 

151.434377 

151.434380 

40 

-309.986003 

-309.986022 

165.164663 

165.164664 

45 

-414.220544 

-414.220565 

168.824984 

168.824985 

50 

-517.304365 

-517.304388 

161.958539 

161.958536 

55 

-611.988644 

-611.988666 

145.422253 

145.422248 

60 

-692.072815 

-692.072834 

121.279843 

121.279843 


Note 


X and Z are expressed in Local Vertical Frame 







Table 4 


OMV Acceleration Along +X Direction 


Time 

in 

Seconds 

X 

in 

meters 

X 

in 

meters 

Z 

in 

meters 

Z 

in 

meters 

.0 

0.000000 

0.000000 

0.000000 

0.000000 

.5 

0.002940 

0.012125 

0.000001 

0.000007 

1.0 

0.012128 

0.024625 

0.000009 

0.000029 

1.5 

0.027565 

0.037125 

0.000032 

0.000065 

2.0 

0.049253 

0.049625 

0.000077 

0.000117 

2.5 

0.077190 

0.062125 

0.000152 

0.000183 

3.0 

0.111377 

0.074624 

0.000263 

0.000264 

3.5 

0.151814 

0.087124 

0.000418 

0.000360 

4.0 

0.198501 

0.099624 

0.000625 

0.000471 


Initial conditions : 

X 1 = X 2 = X 3 = 0 and 

V 1 = v 2 = V 3 = 0 

: All quantities are expressed in Local Vertical Frame. 


Note 





Table 5 


OMV — Pure pitch motion at 0.017453 rad/sec 


KISH 

Pitch 

(Rad) 

■Bl 

Z 

(meters) 

m 

0.0000 

5.0000 

2.4384 

■1 

0.0698 

5.0010 

2.3852 

8 

0.1396 

5.0074 

2.3324 

12 

0.2094 

5.0167 

2.2800 

16 

0.2793 

5.0295 

2.2284 

20 

0.3491 

5.0460 

2.1778 

24 

0.4189 

5.0659 

2.1285 


Note 


All measurements are in flat floor coordinates 
Please see Appendix 1. 







Table 6 


Motion of the Mobile Base under 

constant acceleration of [0.025,0,0]T and constant yaw at 0.08675 rad/sec 


Time 

(Sec) 

X 

(meters) 

Y 

(meters) 

Z 

(meters) 

Yaw 

(rad) 

0 

0.0000 

11.6680 

2.4384 

0.0000 

4 

0.2752 

11.2418 

2.4390 

0.3470 

8 

1.0709 

11.0039 

2.4433 

0.6940 

12 

2.2919 

11.1199 

2.4545 

1.0410 

16 

3.7925 

11.7135 

2.4750 

1.3880 

20 

5.3934 

12.8512 

2.5062 

1.7350 

24 

6.9035 

14.5350 

2.5480 

2.0820 


Note : X, Y and Z are expressed in flat floor coordinates. Please see 

Appendix 1. 




Table 7 
OMV Time Test 


No of Average execution time per major cycle 

Steps Single Precision I Double Precision 


4 

0.077 

0.084 

5 

0.090 

0.099 

6 

0.103 

0.113 

7 

0.117 

0.128 

8 

0.130 

0.143 

9 

0.144 

0.158 

10 

0.157 

0.173 








Table 8 

Optimized OMV Execution Times Per Major Cycle 
As A Function Of Number Of Steps N 


N 


Execution time 


4 

5 

6 

7 

8 
9 


0.068 

0.079 

0.090 

0.100 

0.111 

0.122 


(Sec) 


10 


0.132 




Table 9 


Comparison Test Between N = 4 and N = 10 Steps 


Time 

Solution 

in 

Analytic 

Numeric 

1 

Minutes 



■QBQI 

0 

0.000000 

0.000000 

0.000000 

5 

13.746736 

13.746736 

13.746736 

10 

20.161917 

20.161917 

20.161917 

15 

12.828962 

12.828963 

12.828961 

20 

-12.952953 

-12.952950 

-12.952956 

25 

-59.582233 

-59.582227 

-59.582237 

30 

-126.855544 

-126.855533 

-126.855551 

35 

-211.993191 

-211.993176 

-211.993200 

40 

-309.986022 

-309.986003 

-309.986034 

45 

-414.220565 

-414.220544 

-414.220579 

50 

-517.304388 

-517.304365 

-517.304403 

55 

-611.988666 

-611.988644 

-611.988681 

60 

-692.072834 

-692.072815 

-692.072847 
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Figure 1. MSFC Flatfloor Simulation System 
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OMV Body Frame 
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Hardware incompatible graphics package. 

Vector Transformation Module. See Reference 1. 
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OMV Translational Equations of Motion 



Appendix 1. 


OMV Translational Equations Of Motion 

Consider a target vehicle orbiting the earth with an angular velocity v 
and an orbit radius of R Q . We can define a local vertical frame (LVF) at the 
center of gravity of this vehicle as shown in the figure below : 



Here, X^, and are the three orthogonal axes of the LVF. We can imagine 
that the center of the earth may be considered as the origin of the inertial 
coordinate frame. We can chose the axes of this coordinate system as shown. In 
particular, Yg is parallel to Y^. We shall use the subscript L to denote those 
quantities that are expressed in the LVF, while the subscript E shall be used 
for those quantities expressed in the inertial frame. The point C in the 
above figure represents the center of mass of the chase vehicle (OMV) 



The equation of motion of the chase vehicle is easily deduced from New- 
ton's second law, namely, 

M C R = F g + F c (1) 

This equation is written in the inertial frame. Here, M c is the mass of the 
chase vehicle, F g is the gravitational force exerted on the vehicle by the 
earth, and F c is the control force exerted on the vehicle from the on-board 
thrusters and jets. The objective of this exercise is to derive the equation 
of motion in terms of r and its time derivatives. Namely, we wish to express 
the motion of the chase vehicle (OMV) in local vertical frame. This choice 
turns out to be very convenient for docking maneuvers. 


From the above figure, it is obvious that 

R = R o + r E 

it follows that 

•• • • 4 • 

R = R o + r E 

Since the LVF is a rotating frame, we can use the operator 
{ d/dt }g = { d/dt + w x 

Applying this operator to r twice, we have 


( 2 ) 


( 3 ) 


and 


r E = f L + w x r L 

*fg = d/dt (fg + W X Tg) + W X (fg + w X Tg) 

= + v x + w x + w x (w x Tr ) 

•• ^ , . 

= + 2w x + w x (w x r^) 


( 4 ) 



From equations (3) and (4), we have : 


• « •» • • 

R = R q + r E 

•• •• - 

= R o + r L + 2w x + w x (w x r^) 

Furthermore, for a circular orbit, 

R 0 + w 2 R q = 0 

! 

therefore, 

R = -w z R q + + 2w x + w x (w x r^) (5) 

It is clear at this point that the equations of motion (1) can be rewritten in 
terms of and R Q and their time derivatives. Thus the subscript will be 

dropped from here on. Recall that 

R = R 0 + r 

R 2 = (R q + r) . (Rq + r) 

= R 2 + r 2 + 2R 0 .r 

- R o + 2R o‘ r 

= Ro (1 + 2(R Q .r) / R 2 } 

so that R- 3 = R“ 3 { 1 + 2(R 0 .r) / Rq }~ 3/2 

« R; 3 {1 - 3( R Q.r) / Rq } 

Thus, F g = -(GM e M c / R3 ) R 

= -(GMqMq/Rq ) (R 0 + r) (1 - 3(R 0 .r) / R 2 ) 

= -w 2 H c (R 0 + r) (1 - 3 (Rq . r) / R 2 ) 

~ -w 2 M c (Rq + r - 3(R 0 .r/R 2 )R q (6) 

since for a circular orbit, w 2 = GMg/R 3 . Substituting equations (5) and (6) 



into (1), we have : 


r 

i , 

M c {-w 2 R Q +r+2wxr+wx (wxr)} =F- M c w 2 {R Q + r - 3(R Q .r)/R 2 } 

— If we define A = F c / M c , then we have : 

_ -w 2 R Q + +2wxr + wx(wxr) = A - w 2 R Q - w 2 r + 3w 2 (R 0 .r/R 2 )R 0 

which, after re-arranging, gives : 

*r = A - 2w x r - w 2 r - w x (w x r) + 3w 2 (R 0 .r/R 2 )R Q (7) 

Now, we shall state r, R q and w in cartesian coordinates. It is explicitly 
assumed that the unit vectors i, j and k are directed along X^, and Zj__ axes 
respectively. Thus, 

r = [X, Y, Z] T 

Rq = [0, 0, R q ] T 

w = [0, w, 0] T and 

A = [A x , A y , A z ] T 

and it can easily be shown that : 



2w x r 

= [ 2wZ, 

0, -2wX] T 



w x (w x r) 

= [-w 2 X, 

0, -w 2 Z] T 



3w(R 0 .r/R2)R Q 

= [ o, 

0, 3w 2 Z] T 

and 

- 

w 2 r 

- t w 2 X, 

w 2 Y, w 2 Z] T 


- 

and substituting into equation 

(7) yields 



[X, Y, Z] T 

= [ -2wZ 

, 0, 2wX] t + [ w 2 X, 

0, w 2 Z] T 



+ [ -w 2 X 

, -w 2 Y, -w 2 Z]^ + [ 0, 

0, 3w 2 Z] T 

- 


+ [ A x 

, Ay, A z ] x 




or 


X = A x - 2wZ 

V = A y - w 2 Y (8) 

Z = A z + 2wX + 3 w 2 Z 

Equation (8) is the equation of motion of the chase vehicle relative to the 


target vehicle in local vertical frame 
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INTRODUCTION 


The State Vector Transformation Module (SVX) is an interface between the 
OMV simulation model and the mobile base (TOM_B) of the flat floor simulation 
system. We can imagine the OMV simulation to be a free flying vehicle in space 
under human operator control, and at any particular instant, its state can be 
summarized as a fourteen-component vector called the state vector S. SVX takes 
this state vector as an input and generates an appropriate string of commands 
that is transmitted to T0M_B with the stipulation that if TOM_B executes this 
command string exactly, then the mock-up module mounted on TOM_B will exactly 
replicate the motion of the OMV as percieved by the operator. 

References 1), 2) and 3) are reports that pertain to the various aspects 
of the OMV. From these reports, the various components that make up the state 


vector can be 

deduced and 

are presented below : 

Component 

Symbol 

Meaning 

1 

X 

position of the target vehicle relative 

2 

Y 

to the OMV in local vertical frame LVF. 

3 

Z 


4 

V x 

relative velocity of the chase vehicle 

5 

v y 

in LVF 

6 

V z 


7 

L x 

angular momentum vector in LVF 

8 

L y 


9 

L z 


10 

9l 

attitude quaternions in body frame 

11 

92 


12 

93 



2 


13 


U 

14 ra mass of OMV 

It is often more convenient to consider the state vector to be made up of the 
following four vectors : X = [ X, Y, Z ] T , V = [ V x , V y , V z ] T , L = [ L x , L y , 
L z ] and the unit quaternion q = [ q^, q 2 » qg, q 4 l 1 . 

As mentioned earlier, the required command string must be derived from 
this state vector, and is transmitted to TOM_B as seven 16-bit words. The last 
word can either be a zero or a one, which is interpreted by the TOM_B Executive 
as rate or position control respectively. A brief explanation of the command 
string is shown below : 



Component 

Position control 
symbol menaing 

rate control 
symbol meaning 

coord, system 


1 

y 

yaw of TOM_B 

y 

yaw rate 

body frame 


2 

X 

position of 

V x 

velocity of 

LVF 


3 

Y 

T0MJ3 

v y 

T0M_B 



4 

z 

pos of pivot 

V z 

vel of pivot 


- 

5 

p 

pitch angle 

p 

pitch rate 

body frame 


6 

r 

roll angle 

r 

roll rate 



7 

1 

pos. control 

0 

rate control 



^ Before the detailed analysis is presented, it is necessary to define the 

various coordinate systems used. 

COORDINATE SYSTEMS 

. Several coordinate systems are used in this software module. Specifically, 

motion of the OMV is described in Local Vertical Frame (LVF) while the orienta- 
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tion of the OMV is described in body frame. Similarly, the position and velo- 
city of the mobile base TOM_B is described in floor coordinates while the 
orientation of the mock-up module and TOM_B are described by their respective 
body frames. 

A) the Local Vertical Frame (LVF) 

Imagine a circular orbit that is inclined at an angle i with respect to 
the equatorial plane. A Local Vertical Frame is a non-stationary frame that has 
its origin at a point on this orbit such that 

(i) its Z^ axis is directed away from the earth’s center, 

(ii) its axis is directed tangential to the orbit and is perpendicular 

to its Zg axis, and 

(iii) the Yg axis is directed parallel to the angular momentum vector, as 
shown in Figure 1. 

A subscript L will be used to indicated quantities defined in this coordinate 
system. 

B) the Floor Coordinates (F) 

The floor coordinates has its origin at one corner of the flat floor as 
shown in Figure 2. Its Xp axis is directed along the width of the floor, while 

the Yp a xis is directed along the length of the floor. Naturally, Zp axis is 
directed vertically up. 

C) the TOM_B body Frame (B) 

This coordinate system is fixed with respect to the mobile base, and has 
its origin at the center of mass of the mobile base. Its Xg axis is directed 
towards the front of TOM_B, while its Zg axis is parallel to the Zp axis of the 
flat floor. A third axis Yg is chosen so as to form an orthogonal right-handed 
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coordinate system, a top view of which is shown in Figure 3. 

D) the Mock-Up Module Body Frame (M) 

We shall assume that the mock-up module resembles the OMV in shape (that 
is, not unlike a pancake). The origin of its body frame coincides with its 
center of mass, and the axis is directed towards the front of the module. 
Initially, at the start of the simulation, the Z^ axis is chosen to be parallel 
to Zp, and the appropriate orthogonal axis is chosen as its axis, as in- 
dicated in Figure 4. 

ANALYSIS 

From the references mentioned above, it is obvious that the relative 
position and attitude from the state vector are relative quantities. Thus, 
initial conditions at the start of the simulation must be known. Figures 5 a) 
and b) shows the initial state of the mobile base and mock-up module at the 
start of the simulation. The quantities a, c, 1, h and o can be obtained from 
measurement. 

A necessary initial condition is that the operator must leave the hand 
controllers in the neutral position for at least one second so that the initial 
position of the OMV [ X Q> Y Q , Z Q can be obtained. It is also assumed that 
the initial orientations of both the OMV and mock-up module are set in their 
home position. If the notation r, p, and y is used to indicate the roll, pitch 
and yaw of both the OMV and the mock-up, then, 

[ r oMv> pomv* yomv = t r M» pm» ym ] t = C °» °» 0 ] T 

It is obvious that the corresponding axes of the coordinate frames M, B and F 
are all parallel at this point in time. At any later time, the position of the 
OMV can be calculated from the state vector : 
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[ 0 ] 


S 1 


Here, S^, S2, and S3 are the first three components of the state vector. This 
position is measured relative to the starting point in the beginning of the 
simulation, and can be transformed to the position of the mock-up module in 
floor coordinates using the equation : 



X M 


Y 


~c + 1 - X D - 

y m 

= 

y l 

+ 

a " Y o 

Z M 


. zl . 


h - Z 0 


Equation [I] governs the transformation of the position vector of the OMV in 
LVF to a position vector for the mock-up module in floor coordinates, based on 
the initial conditions and the first three components of the state vector. 
Given that the instantaneous orientation of the module is [ r^, p^, r^ as 
shown in Figure 6 a) and b), the position of T 0 M_B [ Xp, Yp, Zp in floor 
coordinates is given by : 



{c + lcos(p) }cos(y) 
{c + lcos(p) }sin(y) 
5 


[II] 


Note that Zp i s the height of the center of mass of T 0 M_B from the floor (a 
constant quantity), and is not of interest here. In stead, the quantity of 
interest is Z, which is the height of the pivot point from the floor as shown 
in Figure 6, and 

Z = Z M - lsin(p) [III] 

It follows that the velocity of T 0 M_B and the pivot point is given by 
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X F 


+ (c + lcos(p) }sin(p)y + lsin(p)cos(y )p 

Yp 

= 

Yj^j - {c + lcos(p))cos(p)y + lsin(p)sin(y)p 

Z 

. 


Z M - lcos(p)p 


The above transformations take care of the position and velocity quantities. 

The quaternions q^, q 2 , q3, q 4 from the state vector specifies the OMV's 
attitude in body frame, as discussed in References 4) and 5). At any instant, 
its orientation is given by (see Ref 4) : 

[ r, p, y ] T = ct[0 x ,0 y ,0 z ] T [V] 

where 

a = 2 cos"l(q 4 ) 

[ 0 X , 0 y , 0 Z ] T = (iq X + jq 2 + kq 3 ) / (q x + q 2 + q 3 )°* 5 [VI] 

while their rates are wg = [ wj, w 2 , wg]**’ which can be calculated in the 
following manner: 

Since the angular momentum vector L = [ L„, L„, L„ I 1 from the state 
vector is expressed in LVF, it is necessary to transform it to body frame using 
the equation : 

L B = A L [VII] 


here A is the direction cosine matrix which can be constructed from the atti- 
tude quaternions q lt q 2 , q 3 , and q 4 


^4 + Ql ~ <12 ~ 33 
A = I 2(q 1 q 2 - q 3 q 4 ) 
2(qiq 3 + q 2 q 4 ) 


2(qiq 2 + q 3 q4) 

q4 - qi + q2 - q3 

2(q 2 q3 - q^) 


2(qiq3 - q2q4) 

2(q 2 q 3 + q 3 q4) 

q4 - qi - q2 + Q 3 J 

[VIII] 
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Knowing the moment of inertia tensor I, one can calculate the angular rates 

rn 

W B = t w l> w 2* w 3 f 

- I " 1 L b = I " 1 (A L) [IX] 

Thus, one has all the needed information from the state vector to yield the 
necessary position or rate control commands. 

ALGORITHM 

The algorithm for SVX makes use of all the transformations described in 
the above section. Essentially, the algorithm uses the state vector and 
depending on the value of MODE, generates the appropriate command string 
CMDRAW. 

Case 1 MODE <> 0 (position control) 

In this case, both orientation and position of the OMV are updated. A 
transformation is made to yield the position of the center of mass of TOM_B 
using equation [I] through [III]. The orientation of the mock-up module is 
obtained using equation [VI], Using the previous notation, a seven element 
vector 

[ y, X B , Y b , Z, p, r, 1 ] T 

is generated. Each element of this vector is suitably scaled and round off to 
the nearest integer (16-bit word) and is the sole output of the SVX module. 
Rate information is not of interest when the system is in position control, and 
is therefore not transmitted. Throughout this module, the scale factors for all 
angular and displacement quantities are 10^ and 1(P respectively. 
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Case 2 MODE = 0 (rate control) 

In this rate control mode, it is still necessary to update the orientation 
(equation [VI]) although it is no longer necessary to update the position of 
the OMV. The velocity of TOM_B in floor coordinates is determined from equation 
[IV] while the rates for roll, pitch and yaw are determined using equations 
[VII] through [X]. The seven 16-bit word command string is 

[ y. Xg, Yg, Z, p, r, 0 ] 

As before, each component of this vector is similarly scaled and rounded before 
returning. 

Case 3 MODE <> 0 AND MODE <> 1 

In this case, MODE is set to 1, and position control is assumed. 

IMPLEMENTATION 

This algorithm is implemented as a subroutine named SVX (S, CMDRAW, MODE) 
where the three items on the parameter list are the state vector output command 
string and control mode respectively. 

The subroutine is implemented in FORTRAN 77, and the usual programming 
practices are adhered to. Most of the major steps are either properly docu- 
mented in the form of COMMENT statements or implemented as subprograms, fol- 
lowing a modular design approach. Whenever possible, structured codes are used 
unless severe degradation of execution speed may result. 

SVX is compiled and tested using an IBM Personal Computer, and the source 
code, on completion of the testing, is uploaded to the PDP 11/34 computer at 
MSFC. Appendix I shows a complete listing of this module. A more detailed 
description of the testing procedure will be presented later in this section. 
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A local counter (COUNT) is initialized at load time, and updated during 
execution to enable SVX to determine the intial state on strat up. During this 
period, other tasks are carried out as an integral part of the initialization 
process. This include reading a file (SVXINT.DAT) for the values of c, 1, a, h 
and o, as well as the inverse of the moment of inertia tensor I - ^. 

This module assumes that the operator will, at start up, leave the hand 
controller at a neutral position for at least a second. During this interval, 
the initial state of the OMV is recorded, and the vector E where 

E = [ E lf E 2 , E 3 ] T 

= [ c + 1 - X 0 , a - Y 0 , h - Z D ] T 

is calculated. The roll, pitch and yaw of both the OMV and the mock-up module 
are initialized to zero during this process by invoking subroutine ZERO. 

Subsequent calls to SVX causes a seven 16-bit command string in an INTEGER 
array called CMDRAW to be produced. Computation here depends on the value of 
MODE. 

When MODE is non-zero, position control is assumed. SVX invokes subrou- 
tines QTRPY and UPDPOS to calculate the desired orientation and position of the 
OMV. A transformation is then made to determine the required position (of the 
mobile base TOM_B in floor coordinates) and orientation (of the mock-up module 
in body frame). Since the value of MODE cannot be changed in the course of a 
simulation, no rate information is calculated or retained. 

When MODE is zero, rate control is used. First, QTRPY is called to calcu- 
late the orientation of the OMV; its position is not computed because it is not 
of interest while in rate control mode. The direction cosine matrix A is formed 
by invoking subroutine DIRCOS, and a simple matrix multiplication transforms 


10 


the angular momentum to body frame. Finally, the velocity of the OMV (from the 
state vector) is suitably transformed to yield the velocity of TOM_B in floor 
coordinates, and the appropriate command string assembled. 

When MODE is neither zero nor one, it is set to one and defaults to 
position control. One frequently used subroutine in both modes is DECOMP which 
takes the state vector S and decomposes it to form the vectors X, V, L and q 
which correspond to the displacement, velocity, angular momentum and the unit 
quaternion vectors respectively. Throughout this module, no attempt is ever 
made to ensure that the magnitude of q is unity. 

To ensure that SVX generates the correct command string, a series of tests 
were conducted using the IBM PC. First, a simple State Vector Editor is writ- 
ten. This editor allows one to create and edit, interactively, state vectors 
which are placed in sequence in a disk file. Next, a simple main program is 
written and linked to the SVX module. The main program consists of a driver 
loop that reads each state vector from the disk file and invokes SVX. The 
command string outputted by SVX is sent to a printer and the process is re- 
peated until the file of state vectors is exhausted. This simple arrangement 
allows one to verify the correctness of SVX without disturbing it. 

Since it is difficult, if not impossible, to represent the results gra- 
phically in three dimensions, state vectors are chosen such that one can easily 
displays the results in two dimensions. By way of example, a sequence of 60 
state vectors of the form : 

[ 0,0,0, 0,0,0, 0,0,0, 0,0,sin(7.5),cos(7.5), 1500 ] T 

is generated. This set of state vectors simulates 50 seconds of run time in 
which position control is used. The meaning of this state vector is that the 
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OMV is to remain stationary, but executes a yaw at a rate of 15° per major 
cycle (0.1 second). Here, we have assumed that the OMV is a disk shaped object 
having a uniform mass distribution and a constant mass of 1500 pounds. Note 
that in case of position control, the angular momentum vector is inconse- 
quential, so a null vector is used. These figures may not be very realistic, 
but they are adequate for testing the SVX module. Figure 7 shows the result of 
a portion of the output command string. In this and subsequent figures, a 
circle or dot indicates the the position of the center of mass of T0M_B in 
floor coordinates, while an attached arrow shows its yaw. This figure depicts 
that T0M_B moves in a circular path and its yaw is changing at a rate of 15° 
per major cycle. It is noted that the radius of the circular path is equal to 
the distance between the centers of mass of T0M_B and the mock-up module. Thus, 
the mock-up module would be spinning about its axis at the same rate, 
exactly as expected. 

When the state vectors are changed to 

[ 0.5, 0,0, 0,0,0, 0,0,0, 0,0,sin(7.5),cos(7.5), 1500 ] T 

in position control, the path of T0M_B is shown in Figure 8. In this figure, 
T0M_B attempts to move in a circular path with a net displacement of 0.5 feet 
per major cycle. It is easy to conclude that the mock-up module would be 

rotating about its Zjq axis and translate along the Xjq axis simultaneously, as 

\ 

demanded by this state vector. 

CONCLUSION 

Other simular tests have been conducted. For example, the state vector in 
the beginning of this section has been used as input for rate control, and the 
result is plotted in Figure 9. This and simular results have demonstrated that 
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the module SVX is functioning properly and that correct command strings are 
obtained. One must remember that the outputs of this module are commands to 
TOM_B, indicating the desired position, (or velocity) and attitude (or angular 
rates). The proper interpretation, and subsequent execution, of these commands 
are performed by the TOM_B Executive, and is outside the scope of the SVX 
module. 


13 















Fig 5 b) Initial position 
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Figure 6 b) Pitch and roll of Mock-up Module 
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Figure 9 Velocity components of TOM B 
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1 $PAGESIZE: 56 

^ 2 $TITLE: ’<« S V X >»' 

3 C 

4 C 

- 5 C STATE VECTOR TRANSFORMATION MODULE (SVX) 

6 C 

7 C by 

8 C 

9 C 

10 C Dr. W. Teoh 

11 C 

12 C UAH 1984 

13 C 

14 C 

15 C 

16 C 

17 SUBROUTINE SVX (S, CMDRAW, MODE) 

18 C 

19 C 

20 C 

21 C 

- 22 C This is the state vector transformation module which accepts a 

23 C 14 element state vector S of the OMV as input and generates a 

24 C 6-element command string CMDRAW as output. The argument MODE 

_ 25 C conveys the following meaning : 

26 C 



27 C 

MODE 


Meaning 


28 C 

0 


rate control 


29 C 

1 


position control 


30 C 

anything else 

defaults to 1 


31 C 




- 

32 C 

Summary of the 

state 

vector components are as follows : 


33 C 





34 C 

Component 


Meaning 


35 C 





36 C 

1 

X 

position of target vehicle from the 


37 C 

2 

Y 

chase vehicle in LVF 


38 C 

3 

Z 



39 C 

4 

VX 

relative velocity of the two vehicles 


40 C 

5 

VY 

in LVF 


41 C 

6 

VZ 


- 

42 C 

7 

LX 

angular momentum vector in LVF 


43 C 

8 

LY 



44 C 

9 

LZ 



45 C 

10 

Q1 

attitude quaternions in body frame 


46 C 

11 

Q2 



47 C 

12 

Q3 



48 C 

13 

Q4 


" 

49 C 

14 

M 

instantaneous mass in kg. 



; x s v x 
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Line# 

1 


50 

C 


51 

C 


52 

C 


53 

C 


54 

C 


55 

C 


56 

C 


57 

C 

— 

58 

C 


59 

c 


60 

c 


61 

c 


62 

c 


63 

C 


64 

c 


65 

c 


66 

c 


67 

c- 

— 

68 

c 


69 

c 


70 



71 



72 



73 



74 



75 



76 



77 


- 

78 

c 


79 



80 

c 


81 

c 


82 

c 


83 



84 

c 


85 

c 


86 

c 


87 


— 

88 



89 

c 


90 

c 


91 

c 


92 



93 

c 


94 

c 

- 

95 

c 


96 



97 



98 
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Summary of command string 


component 

meaning 

1 

YAW 

2 

X 

3 

Y 

4 

Z 

5 

PITCH 

6 

ROLL 

7 

MODE 


components: 

coord system 
body frame 
floor coordinate 
floor coordinate 
floor coordinate 
body frame 
body frame 
integer 


This module maintains a local counter to process initial 
conditions at the start of the simulation. 


REAL * 8 S(14) 

REAL * 8 X(3) , V(3), L(3), Q(4) 

REAL * 8 X0(3) , XM(3), E(3), XH0LD(3) 

REAL * 8 IINV(3) , LB(3), W(4) 

REAL * 8 RPY(3) , QD0T(4), QW(4,4), A(3,3) 

REAL * 8 LL, UL, UA, CC, AA, HH, QQ, TX, TY, Z 

REAL * 8 ROLL, PITCH, YAW, ROLDOT, PITDOT, YAWDOT 

REAL * 8 Ql, Q2, SY, CY, VX, VY, VZ 

INTEGER CMDRAW(7) , COUNT, MODE 


***' load-time initialization 


DATA COUNT /0/ 

*** decompose state vector and process it 

CALL DECOMP (S, X, V, L, Q) 

IF (COUNT .NE. 0) GOTO 300 

*** initialization before start 

CALL ZERO (XO, 3) 


*** read parameters 

OPEN (1, FILE = ’SVXINT.DAT', STATUS = ’OLD') 
READ (1, 20) CC, LL, AA, HH 
READ (1, 20) IINV 
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99 C 


p*" 

100 C 

-■“i 5 * calculate inverse of moment of inertia tensor 


101 C 



102 

DO 50 K = 1, 3 

-1 

103 

IINV(K) = 1.0 / IINV(K) 

1 

104 50 

CONTINUE 


105 

CLOSE (1) 


106 C 



107 C 

*** set conversion factors 


108 c 



109 

UL = 10000.0 


110 

UA = UL 


111 

COUNT = COUNT + 1 


112 C 



113 C 

*** set transformation matrix elements to floor coord. 


114 C 



115 

E( 1 ) = CC + LL - X0(1) 


116 

E(2) = AA - X0(2) 


117 

E(3) = HH - X0(3) 


118 C 

j 


119 C 

*** initialize to home orientation 

— 

120 C 



121 

CALL ZERO (RPY, 3) 


122 

COUNT = COUNT + 1 


123 C 



124 300 

IF (MODE .NE. 1) GO TO 400 


125 C 



126 C 

*** position commands 


127 C 



128 C 

*** update orientation and position 


129 C 


- 

130 

CALL QTRPY (Q, ROLL, PITCH, YAW) 


131 

CALL UPDPOS (XM, X, XHOLD, E, 3) 


132 C 



133 C 

*** set orientation part of the command string 


134 C 



135 

CMDRAW(7) = 1 


136 

CMDRAW(6) = JFIX(ROLL * UA) 

- 

137 

CMDRAW(5) = JFIX( PITCH * UA) 


138 

CMDRAW(l) = JFIX(YAW * UA) 


139 C 



140 C 

*** transform to T0M_B position in floor coordinates 


141 C 



142 

QQ = CC + LL * DC0S( PITCH) 


143 C 



144 C 

*** X-component 


145 C 



146 

TX = XM(1) - QQ * DCOS(YAW) 

- 

147 

CMDRAW(2) = JFIX (TX * UL) 



U S V X »> 


( 

i .ine# 1 
148 C 

r 149 c 

150 C 

151 
_ 152 

153 C 

154 C 
_ 155 C 

156 

157 

158 C 

- 159 C 

160 C 

161 C 

- 162 

163 C 

164 400 

165 C 

- 166 C 

167 C 

168 

- 169 C 

170 C 

171 C 

_ 172 C 

173 

174 

175 C 

~ 176 C 

177 C 

178 

- 179 
180 

181 C 

182 C 

183 C 

184 

185 

~ 186 

187 

188 C 

- 189 C 

190 C 

191 

192 

193 

194 

195 C 

~ 196 C 
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*** Y-component 

TY = XM(2) - QQ * DSIN(YAW) 

CMDRAW(3) = JFIX (TY * UL) 

*** Z-component 

Z = XM(3) - LL * DSIN(PITCH) 

CMDRAW(4) = JFIX (Z * UL) 

*** This is a good place to call the 1/0 driver to 
*** transmit to T0M_B, but we won't for now 

RETURN 

IF (MODE .NE. 0) GO TO 900 
*** rate control 
CALL QTRPY (Q, ROLL, PITCH, YAW) 

*** form direction cosine matrix and calculate angular 
*** momentum in body frame 

CALL DIRC0S (A, Q) 

CALL MMUL (A, L, LB, 3) 

*** compute body rate 

R0LD0T = IINV(l) * LB(1) 

PITD0T = IINV(2) * LB(2) 

YAWDOT = IINV(3) * LB(3) 

*** construct orientation part of command string 
CMDRAW(7) = 0 

CMDRAW(6) = JFIX (R0LD0T * UA) 

CMDRAW(5) = JFIX (PITDOT * UA) 

CMDRAW(l) = JFIX (YAWDOT * UA) 

*** compute velocity of TOM_B in floor coordinates 

Q1 = LL * DSIN( PITCH) * PITDOT 

Q2 = (CC + LL * DC0S( PITCH)) * YAWDOT 

SY = DSIN(YAW) 

CY = DCOS(YAW) 




X-component of velocity in floor coordinate 
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197 

C 





r 198 


VX = V(l) + Q1 * 

CY + Q2 * SY 



! . 199 


CMDRAW(2) = JFIX 

(VX * UL) 



200 

C 





r- 201 

c 

*** Y-component 

of velocity in floor 

coordinate 

202 

c 





203 


VY = V(2) + Q1 * 

SY - Q2 * CY 



__ 204 


CMDRAW(3) = JFIX 

(VY * UL) 



205 

c 





206 

c 

*** Z-component 




207 

c 





~ 208 


VZ = V(3) - LL * 

DC0S( PITCH) * PITDOT 



209 


CMDRAW(4) = JFIX 

(VZ * UL) 



210 


RETURN 




- 211 

c 





212 

900 

CONTINUE 




213 

C 





214 

C 

*** We have an un-recognizable code, 

default 

to 1 for 

215 

c 

*** position control 



216 

c 





217 


MODE = 1 




218 


GO TO 300 




219 

c 





220 

10 

FORMAT (4F10.2) 




221 

20 

FORMAT ( F15.8) 




222 


END 





_Name 

Type 

Offset 

P 

Class 

A 

REAL*8 

466 



AA 

REAL*8 

558 



-cc 

REAL*8 

542 



3MDRAW 

INTEGER*4 

4 

* 


COUNT 

INTEGER*4 

538 



-CY 

REAL*8 

698 



JCOS 




INTRINSIC 

JSIN 

E 

REAL* 8 

418 


INTRINSIC 

1H 

REAL*8 

566 



[INV 

REAL*8 

442 



K 

INTEGER*4 

574 




REAL*8 

370 



i 

REAL*8 

394 



lL 

REAL*8 

550 



MODE 

INTEGER*4 

8 

* 


3 ITCH 

REAL*8 

602 



PITDOT 

REAL*8 

658 



Q 

REAL*8 

154 



11 

REAL*8 

674 
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ine# 1 


2 

REAL*8 

682 

P® T 

REAL*8 

210 

C 

REAL*8 

618 

W 

REAL*8 

242 

QLDOT REAL*8 

650 

( L 

REAL*8 

594 

i X 

REAL*8 

186 


REAL*8 

0 


REAL*8 

690 


REAL*8 

626 

Y 

REAL*8 

634 

X - 

REAL*8 

586 


REAL*8 

578 


REAL*8 

98 

JL 

REAL*8 

706 


REAL*8 

714 

i-j 

REAL*8 

722 

f 

REAL* 8 

122 


REAL*8 

2 


REAL*8 

26 

HOLD 

REAL*8 

74 

V- 

REAL*8 

50 

1 V 

REAL*8 

610 

aWDOT 

REAL*8 

666 

• 

REAL*8 

642 
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224 

C 





> — 1 

225 

C 






226 


SUBROUTINE 

DECOMP (S, X, 

V, L, Q) 


227 

c 





, 

228 

c 


— 

— 



229 

c 






230 

c 

This procedure decomposes 

the State vector S into its components 


231 

c 

which are 

also vectors. They have the following meaning : 

i 

232 

c 






233 

c 

Vector 


Dimension 

Meaning 


234 

c 

X 


3 

Position vector in LVF 

n 

235 

c 

V 


3 

Velocity vector in LVF 


236 

c 

L 


3 

Angular momentum in LVF 


237 

c 

Q 


4 

Unit quaternion in body frame 


238 

c 






239 

c 


— 




240 

c 






241 


REAL * 

8 

S( 14) , X(3) , V(3), L(3) , Q(4) 

_ 

242 

c 






243 


CALL 

LD 

(S, X, 1, 3) 



244 


CALL 

LD 

(S, V, 4, 3) 


— 

245 


CALL 

LD 

(S. L, 7, 3) 



246 


CALL 

LD 

(S, Q, 10, 4) 



247 

c 






248 


RETURN 





249 


END 





Name Type 

REAL*8 
REAL*8 
REAL*8 
REAL*8 
REAL*8 


Offset P Class 

12 * 

16 * 

0 * 

8 * 

4 * 
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251 C 
r 252 C 

253 SUBROUTINE LD (A, B, M, N) 

254 C 

— 255 C ~ 


256 C 



257 C 

This procedure copies N elements of vector A to vector B, 

258 C 

starting at the M-th element 

r 259 C 



260 C 

— 


261 C 



r 262 


REAL * 8 A( 14) , B(N) 

263 


DO 100 K = 1, N 

264 


B(K) = A(M + K - 1) 

265 100 


CONTINUE 

266 


RETURN 

267 


END 

~~ne Type 


Offset P Class 

REAL*8 


0 * 

REAL*8 


4 * 

INTEGERS 

750 

INTEGER*4 

8 * 

INTEGER*4 

12 * 


268 $PAGE 
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D Line# 1 7 

269 C 

270 C 

271 SUBROUTINE MMUL (A, B, C, N) 

272 C 

273 C 

274 C 

275 C This procedure performs a matrix multiplication of an NxN 

276 C matrix A to an N-element column matrix B to yield an N- 

277 C element column matrix C 

278 C 

279 C— 

280 C 

281 

282 C 

283 
1 284 

1 285 

2 286 

2 287 200 

1 288 
1 289 100 

290 

291 


REAL * 8 A(N,N), B(N) , C(N), S 

DO 100 I = 1, N 
S = 0.0 

DO 200 J = 1, N 

S = S + A(I,J) * B(J) 
CONTINUE 
C(I) = s 
CONTINUE 
RETURN 
END 
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^Name 

Type 

A 

REAL*8 

B 

REAL*8 

-c 

REAL*8 

I 

INTEGER* 4 

J 

INTEGER*4 

-N 

INTEGER*4 

S 

REAL*8 


Offset P Class 

0 * 

4 $ 

8 * 

758 
774 
12 * 

766 


292 $PAGE 


>\n 


»> 


S 


) Ane# 
293 
- 294 

295 

296 
_ 297 

298 

299 

300 

n 301 

302 

303 
r- 304 

305 

306 
_ 307 

308 

309 


V X 


1 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 


100 


7 
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SUBROUTINE ZERO (A, N) 


This procedure initializes an N-element array A to zero at 
run time 


REAL * 8 A(N) 
DO 100 K - 1, N 
A(K) = 0.0 
CONTINUE 
RETURN 
END 


- 'ne Type 

REAL*8 

INTEGERS 

INTEGER*4 


Offset P Class 

0 * 

782 
4 * 


310 $PAGE 
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D Line# 1 
311 C 
~ 312 C 

i . 313 

314 C 

- 315 C— 

1 316 C 

317 C 
_ 318 C 

319 C 

320 C 

321 C 
r 322 C 

323 C— 

324 C 

- 325 C 

326 

327 C 

328 

329 
. . 330 

1 331 100 

332 

333 
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SUBROUTINE UPDPOS (XM, X, XHOLD , E, N) 


This procedure updates the position of the 0MV in local vertical 
frame (XHOLD). 

The new position of the module in floor coordinates is then com- 
puted (XM) 


REAL * 8 XM(N), X(N), XHOLD(N) , E(N) 

DO 100 K = 1, N 
XHOLD(K) = X(K) 

XM(K) = XHOLD(K) + E(K) 

CONTINUE 

RETURN 

END 


-Name Type 


Offset P Class 


i J 

REAL*8 

12 

❖ 

K 

INTEGERS 

790 



INTEGER*4 

16 

* 


REAL*8 

4 

* 

XHOLD 

REAL*8 

8 

* 

"M 

REAL*8 

0 

T* 


334 $PAGE 



S V X 


i .ine# 1 
335 C 

- 336 C 
337 

' 338 C 

339 

340 

341 

342 

- 343 

344 

345 


C- 

C 

C 

C 

C 

C- 

C 


»> 
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INTEGER FUNCTION JFIX (RR) 


This procedure properly rounds a real number R to the nearest 
integer. 


_ 346 

REAL * 8 RR 

i 347 

REAL R 

1 348 

R = RR 

349 

IF (R .GE. 0) THEN 

r 1 350 

JFIX = IFIX (R + 

351 

ELSE 

352 

JFIX = IFIX (R - 

- 353 

END IF 

354 

RETURN 

355 

END 

me Type 

Offset P Class 

:fix 

INTRINSIC 

REAL 

798 

! REAL*8 

0 * 


356 $PAGE 



«< S V X »> 


D Line# 
357 


1 

”2 

2 

2 

-2 

2 

2 

_2 

1 

.1 


C 

C- 

C 

C 

C 

c 

c 

c 

c 

c- 

c 


358 

359 

360 

361 

362 

363 

364 

365 

366 

367 

368 

369 

370 

371 

372 

373 

374 

375 

376 

377 

378 

379 

380 

381 

382 

383 100 

384 


110 
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SUBROUTINE SETQ (QW, Q) 


This procedure constructs a 4x4 transformation matrix QW from 
the attitude quaternions Q 

For reference, please see "Software Specifications For Docking 
Simulation Of The 0MV" by J. Micheals, January, 1984. 


REAL * 8 QW(4,4) , Q(4) 

DO 100 I = 1, 3 

DO 110 J = 1+1, 4 
KK = I + J 
K = KK - (KK/4) * 4 
IF (K .EQ. 0) K = 2 
ISGNN = 1 

IF ((J .EQ. 1+1) .AND. (J.NE. 4)) ISGNN = 
QW(I, J) = ISGNN * Q(K) 

CONTINUE 
QW(I,I) = Q( 4) 

CONTINUE 
QW(4,4) = Q( 4) 


-1 



385 C 

386 DO 

200 I = 2, 4 

1 

387 

KK = I - 1 

-1 

388 

DO 200 J = 1, KK 

2 

389 

QW(I,J) = -QW(J 

2 

390 200 CONTINUE 

391 RETURN 

392 END 

Name Type 

Offset P Class 

1 

INTEGERS 

802 

ISGNN INTEGERS 

818 

J 

INTEGER-4 

806 

r 

INTEGER*4 

814 

.:k 

INTEGERS 

810 

Q 

REAL*8 

4 * 

")w 

REAL*8 

0 * 
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s 


iine# 


394 
- 395 
396 
* 397 

398 
r 399 
. . 400 
401 
p 402 
403 


404 


_ 405 
! 406 

[ 407 

408 
~ 409 

410 

411 
- 412 

413 

414 


L 415 

416 

417 
! 418 
“ 419 

420 
! 421 

— 422 

423 
:■ 424 

I 425 

426 

427 


! 428 

429 

430 
! 431 
L 432 

433 
. 434 

> 435 

" 436 

437 


V X 


1 

C 

C 

c 

c- 

c 

c 

c 

c 

c- 

c 

c 

c 

c 

c 

c 


c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 
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SUBROUTINE DIRCOS (A, Q) 


This procedure takes the quaternion vector and generates 
a 3 X 3 direction cosine matrix A 


REAL * 8 Q(4), A(3,3), QKS, QRS, SI 

DO 100 K = 1, 3 

*** initialize diagonal elements 

A(K,K) = Q(4) ** 2 
DO 100 J = 1, 3 

*** fix up the diagonal elements 


A(K,K) = A(K,K) + DLTKRK(K.J) * Q(J) ** 2 

*** now do the off-diagonal elements 

IF ( J .GT. K ) THEN 

*** calculate index I <> J & K 

I = 6 / (J * K) 

*** calculate the proper sign 

SI - QSIGN (K, J) 

QKJ = Q(K) * Q( J) 

QRS = Q(I) * Q(4) * SI 
A(K, J) = 2.0 * (QKJ + QRS) 

A(J,K) = 2.0 * (QKJ - QRS) 

END IF 
CONTINUE 
RETURN 
END 


^rae Type 


Offset P Class 

0 * 

838 


l 

L 


REAL*8 

INTEGER*4 



‘ «< S V X »> 


. . D Line# 1 7 


J 

INTEGER*4 

830 

- K 

INTEGER*4 

826 

Q 

REAL*8 

4 

' QKJ 

REAL 

854 

r-QKS 

REAL*8 


! QRS 

REAL*8 

858 

1 SI 

REAL*8 

842 

r 
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S V X 


'■^ine# 1 

439 C 

440 C 

441 

442 C 
- 443 C 

| 444 C- 

1 445 C 

446 C 
n 447 
. 448 

449 
r 450 

451 

452 
_ 453 
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REAL FUNCTION DLTKRK (K f J) 


REAL S 

INTEGER K, J 

S = 1.0 

IF (K .NE. J) S = -1.0 

DLTKRK = S 

RETURN 

END 


aine Type 


Offset P Class 


INTEGER*4 4 * 
INTEGERS 0 * 
REAL 866 
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455 

456 

1 , 457 

c 

c 

REAL 

FUNCTION QSIGN(K.J) 



458 C 

459 C 

460 C 

461 C 

462 C 

463 S = 1.0 

464 L = J + K 

465 IF (M0D(L,2) .EQ. 0) S = -1.0 

466 QSIGN = S 

467 RETURN 

468 END 

Name Type Offset P Class 

INTEGER*4 4 * 

INTEGERS 0 * 

INTEGER* 4 874 

MOD INTRINSIC 

- S REAL 870 
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• jine# 1 7 

Microsoft FORTRAN77 V3 

470 C 
- 471 C 

472 

SUBROUTINE QTRPY (Q, R, P, Y) 

' 473 C 

474 C 

i 475 C 

This subroutine calculates a reasonable set of roll 

I ■ 476 C 

pitch and yaw from the quaternion Q 

477 C 
- 478 C 

479 

REAL * 8 Q(4) , R, P, Y, M, THETA, CA, CB, CG 

480 C 

' 

_ 481 

M = DSQRT (Q(l)**2 + Q(2)**2 + Q(3)**2) 

482 C 

483 C 

calculate direction cosines CA, CB, CG 

484 C 

485 

IF (DABS(M) .LE. 1.0D-20) THEN 

486 

CA = 0.0 

487 

CB = 0.0 

- 488 

CG = 0.0 

489 

ELSE 

490 

CA = Q( 1) / M 

_ 491 

CB = Q( 2) / M 

492 

CG = Q(3) / M 

493 

END IF 

494 C 

“ 495 C 

calculate angle of rotation about Euler axis 

496 C 

497 

THETA = 2.0 * DAC0S(Q(4)) 

- 498 C 

499 C 

now determine the roll, pitch and yaw 

500 C 

501 

R = CA * THETA 

502 

P = CB * THETA 

503 

Y = CG * THETA 

504 

RETURN 

- 505 

END 

fame Type 

Offset P Class 

A REAL-8 

886 

,B REAL*8 

894 

:g REAL*8 

902 

"abs 

INTRINSIC 

.ACOS 

INTRINSIC 

)SQRT 

INTRINSIC 

REaL*8 

878 

REAL-8 

8 * 

3 REAL-8 

0 * 

1 REAL*8 

4 * 
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THETA REAL*8 910 

Y REAL*8 12 * 


Page 19 
07-14-84 
13:01:57 

Microsoft F0RTRAN77 V3.13 8/05/83 


506 $PAGE 



,ine# 1 


13:01:57 

Microsoft F0RTRAN77 V3.13 8/05/83 


ne 

Type 

Size Class 

ECOMP 


SUBROUTINE 

ptcos 


SUBROUTINE 

; ‘KRK 

REAL 

FUNCTION 

r IX 

INTEGERS 

FUNCTION 

D 


SUBROUTINE 

“JL 


SUBROUTINE 

.. IGN 

REAL 

FUNCTION 

TRPY 


SUBROUTINE 

“TQ 


SUBROUTINE 

( 


SUBROUTINE 

PDPOS 


SUBROUTINE 

ERO 


SUBROUTINE 


Pass One 


No Errors Detected 
506 Source Lines 
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0 M V PLOT >»’ 


4 C- 

5 C 

6 C 

7 C 

8 C 

9 C 


10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 


C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C- 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c- 

c 

c 

c 

c 

c 

c 

c- 

c 

c 

c 


Program : 


0 M V P L 0 T 


by 

Dr. W. Teoh 
UAH 1984 


This is a graphical package that accepts a command string 
and uses this information to generate and display the 
position and orientation of T0M_B and the attached mock- 
up module in two dimensions. One can choose to display 
either the top or side view of the system. 

This package is developed in FORTRAN 77 to run on an IBM 
PC with at least 128K of RAM, and fitted with a TECMAR 
GRAPHICS MASTER board. An IBM Monochrome monitor is used 
for the actual display. The resolution in this work is 
chosen to be 640 x 350. 


SUBROUTINE SIDEVEW (H, X, P) 


This procedure produces a side view of T0M_B and the 
attached mock-up module. The perspective is always in 
the direction of +1 axis of the body fixed coordinate 
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50 C 

system 



p 

51 C 





52 C 





53 C 

. 



r 

54 C 





55 C 





56 

REAL * 8 

H, X, P, C, S 



57 

REAL 

XF0RM(3,3) , SDF0RM(3,3) , V0(3,10), 

V(3,10) 


58 

REAL 

R0T(3,3) , FL00R(3,3) , Vl(3,10) 



59 

REAL 

CC, DD, LL, RR, WW, TT 



60 

INTEGER 

FLAG, N, CLR, EF , EEF , PRTFG 


— 

61 C 





62 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 



63 

COMMON /MF/ XFORM , SDFORM, VO, VI 



64 

COMMON /ME/ EF, EEF, PRTFG 



65 C 





66 C 





67 

N = 10 




68 

AA = 1.0 




69 C 





70 C 

*** define mock-up module shape at origin 


— 

71 C 





72 

DO 100 K = 

1, N 


l 

73 

V(3, K) 

= 1.0 


_1 

74 100 

CONTINUE 




75 C 


« point A » 



76 

V(l,l) = 

TT 



77 

V(2,l) = ■ 

-DD 



78 C 


« point B » 



79 

V(1 ,2) =- 

-TT 



80 

V(2,2) = - 

-DD 


- 

81 C 


« point C » 



82 

V(1 ,3) =- 

-TT 



83 

v V(2,3) = 

DD 



84 C 


« point D >> 



85 

V(l,4) = 

IT 



86 

V(2,4) = 

DD 



87 C 




” 

88 C 

*** rotate it by P radians 



89 C 





90 

CALL SINCOS (P, S, C) 


- 

91 

CALL NOTHNG (ROT, 3) 



92 

ROT (1,1) = 

C 



93 

ROT (1,2) = 

-S 



94 

R0T(2,1) = 

S 



95 

ROT (2,2) = 

C 



96 

CALL XMUL 

(ROT, V, 4) 



97 C 




- 

98 C 

*** calculate translation 




\< 

0 
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: Line# 

1 


99 

C 

r 

100 



101 


* 

102 

c 


103 

c 


104 

c 


105 



106 


m *- 

107 



108 



109 

c 

— 

110 

c 


111 

c 


112 

c 


113 

c 


114 



115 



116 

c 

— 

117 



118 



119 

c 

— 

120 



121 



122 

c 


123 



124 



125 

c 


126 


— 

127 



128 

c 


129 


mm 

130 



131 

c 


132 

c 


133 

c 

~ 

134 



135 



136 


- 

137 

c 


138 

c 


139 

c 


140 



141 

c 


142 

c 


143 

c 

— 

144 



145 



146 


- 

147 
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PX = CC + LL * C 
PY = H + LL * S 

*** move the rotated module out there 


CALL NOTHNG (ROT, 3) 
ROT (1,3) = PX 
ROT (2,3) = PY 
CALL XMUL (ROT, V, 4) 


*** now calculate the shape of the base 


XX = X 

+ CC 

« 

point 

E 

» 

V(l,5) 

= CC 





V(2,5) 

= H 

« 

point 

F 

» 

V( 1 ,6) 

= CC 





V(2,6) 

= AA 

« 

point 

G 

» 

V( 1 ,7) 

= CC 





V(2,7) 

= 0. 

« 

point 

H 

» 

V(l,8) 

= -RR 





V(2,8) 

= 0. 

« 

point 

I 

» 

V(l,9) 

= -RR 





V(2,9) 

= AA 






V(l,10) = PX 
V(2, 10) = PY 


*** Transform to floor coordinates 

CALL NOTHNG (FLOOR, 3) 

FL00R(1 ,3) = X 

CALL XMUL (FLOOR, V, N) 

*** transform to screen coordinates 

CALL XMUL (SDFORM, V, N) 

*** erase old picture 

CALL DRWFLR (VO) 

IF ((EF .EQ. 0) .AND. (EEF .NE. 0)) THEN 
CLR = 0 

CALL SDRAW (VI, N, CLR) 



' «< 

0 M V P L 

0 T »> 


• D Line// 1 7 




148 

END IF 


r 

149 

CLR = 1 


! 

150 

CALL SDRAW (V, 

N, CLR) 


151 

CALL MOVE (V, 

VI, N) 

, , 

152 

EEF = 1 



153 C 




154 

RETURN 


p_ 

155 

END 


Name 

Type 

Offset P Class 


“«lA 

REAL 

198 



REAL*8 

218 


CC 

REAL 

4 /MG 

/ 

_CLR 

INTEGER*4 

234 


OD 

REAL 

8 /MG 

/ 

ZEF 

INTEGER*4 

4 /ME 

/ 

EF 

INTEGERS 

0 /ME 

/ 

"RLAG 

INTEGER*4 

0 /MG 

/ 

rLOOR REAL 

158 


II 

REAL* 8 

0 * 


-x 

INTEGER* 4 

202 


LL 

REAL 

12 /MG 

/ 

N 

INTEGER*4 

194 


_P 

REAL*8 

8 * 


PRTFG INTEGERS 

8 /ME 

/ 

PX 

REAL 

226 


PY 

REAL 

230 


"ROT 

REAL 

122 


RR 

REAL 

16 /MG 

/ 

S 

REAL* 8 

210 


-SDFORM REAL 

36 /MF 

/ 

rr 

REAL 

24 /MG 

/ 

V 

REAL 

2 


JO 

REAL 

72 /MF 

/ 

/I 

REAL 

192 /MF 

/ 

VW 

REAL 

20 /MG 

/ 

X 

REAL*8 

4 * 


JFORM REAL 

0 /MF 

/ 
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157 
r- 158 
! 159 

' 160 
- 161 
162 

163 

164 
~ 165 

166 

167 

- 168 

169 

170 
_ 171 

172 

173 

174 
~ 175 

176 

177 

- 178 

179 

180 
181 
182 

183 

184 
~ 185 

186 

187 

- 188 

189 

190 

191 

192 

193 

194 
- 195 

196 

197 
_ 198 


M V 


1 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c- 

c 

c 


c 

c 

c 

c 

c 

c 


c 

c 

c 


c 

c 
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SUBROUTINE SDRAW (V, N, CLR) 


This procedure draws the side view of T0M_B 


REAL V(3, 10) 

INTEGER N, CLR, XI, X2, Yl, Y2 

*** draw mobile base 

CALL RCT (V, 5, CLR) 

*** draw linkage 

XI = V(l,6) 

Yl = V(2,6) 

X2 = V(l, 5) 

Y2 = V(2,5) 

CALL LINE (XI, Yl, X2, Y2, CLR) 

XI = V( 1 , 10) 

Yl = V(2, 10) 

CALL LINE (X2, Y2, XI, Yl, CLR) 


*** draw mock-up module 


CALL RCT (V, 0, CLR) 
CALL PURGE 
CALL GRFRDY 

CALL HOME 

RETURN 

END 


ame Type 

"R INTEGERS 
INTEGER-4 
REAL 

INTEGERS 


Offset P Class 

8 * 

4 * 

0 * 

238 


a 
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12 

INTEGERS 

246 

r-Yl 

INTEGERS 

242 

1 Y2 

INTEGER-4 

250 
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SUBROUTINE RCT (V, OFF, CLR) 



203 C 



204 C 



205 C 


' 

206 C 


r _ i, 

207 C 

This procedure i 


208 C 



209 C 



210 C 



211 C 


[ _ 

212 C 



213 

REAL V(3 


214 

INTEGER OFF 


215 C 



216 

DO 100 K = 1, 4 


217 

J = K + OFF 


218 

X(K) = V(1,J 


219 

Y(K) = V(2, J 


220 100 

CONTINUE 

— 

221 

CALL P0LYGN(4, 


222 

RETURN 


223 

END 

me Type 

Offset P Class 

:lr 

INTEGER-4 

8 * 


INTEGER-4 

338 


INTEGERS 

334 

)FF 

INTEGERS 

4 * 


REAL 

0 * 


INTEGER-4 

254 

r 

INTEGER* 4 

294 


224 $PAGE 
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D Line# 
_ 225 


l 


r 


i 


c- 

c 

c 

c 

c 

c 

c 

c 

c 

c- 

c 

c 


226 C 

227 C 

228 

229 

230 

231 

232 

233 

234 

235 

236 

237 

238 

239 

240 

241 

242 

243 

244 

245 

246 

247 

248 

249 

250 

251 

252 

253 

254 

255 

256 

257 

258 

259 

260 
261 
262 

263 

264 


ame 

CC 

°MD 

D 

l'LAG 

H 
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This is the plot part of the graphical package, and can 
be directly callable from OMV or SVX. The value of FLAG 
obtained from the disk file named SIZE.DAT dictates one 
of top or side view to be displayed. 


Type 

REAL 

INTEC-ER-4 

REAL 

INTEGERS 

REAL*8 

REAL 


INTEGER CMD(7), FLAG 

REAL * 8 X, Y, T, UL, UA, II 

REAL XF0RM( 3 , 3 ) , SDF0RM( 3,3) ,CC,LL,DD,RR,WW,TT 

REAL V0(3, 10) , Vl(3, 10) 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM , SDFORM, VO, VI 

UL = 10000.0 
UA = UL 

IF (FLAG .EQ. 0) THEN 

T = CMD(l) / UA 

X = CMD(2) / UL 

Y = CMD(3) / UL 

CALL TOPVEW (X, Y, T) 

ELSE 

II = CMD (4) / UL 
X = CMD(2) / UL 
T = CMD(5) / UA 
CALL SIDEVEW (H, X, T) 

END IF 

RETURN 

END 

Offset P Class 


4 /MG 
0 * 


8 

0 

382 

12 


/MG 

/MG 


/ 

/ 


/MG / 
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L ine# 1 


1 

REAL 

16 

/MG 

/ 

«0RM 

REAL 

36 

/MF 

/ 

| 

REAL* 8 

358 



r ' 

REAL 

24 

/MG 

/ 

L. 

REAL*8 

350 




REAL*8 

342 



J 

REAL 

72 

/MF 

/ 

l_ 

REAL 

192 

/MF 

/ 


REAL 

20 

/MG 

/ 


REAL*8 

366 



FORM 

REAL 

0 

/MF 

/ 

H' 

REAL*8 

374 
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267 
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268 


269 

C 

270 

C- 

271 

C 

272 

C 

273 

C 

274 

C 

275 

C 

276 

C 

277 

C- 

278 

C 

279 


280 


281 


282 


283 


284' 

C 

285 


286 


287 


288 

C 

289 


290 

C 

. 291 

C 

292 

C 

293 


294 

C 

295 

C 

296 

C 

297 

C 

298 

C 

299 


300 


301 


302 


303 


304 


305 

C 

306 


307 

C 

308 

C 

309 

C 

310 

c 

311 


312 


- 313 


314 
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SUBROUTINE TOPVEW (PX, PY, THETA) 


This procedure constructs the top view of T0M_B. No 
correction to perspective distortion is implemented 


REAL * 8 PX, PY, THETA, S, C 
REAL V(3,10) , V0(3,10), SDF0RM(3,3) 

REAL R0T(3,3) , FL00R(3,3), XF0RM(3,3) 

REAL CC, DD, LL, RR, WW, TT, Vl(3,10) 

INTEGER FLAG, N, CLR, EF, EEF, PRTFG 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM , SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 

N = 10 


*** get TOM_B shape at the origin 
CALL ORGPOS (V, N) 

rotate by THETA if needed 

a? 

IF (THETA .NE. 0.0) THEN * 

*** construct rotation matrix 
CALL NOTHNG (ROT, 3) 

CALL SINCOS (THETA, S, C) 

R0T(1 ,1) = C 

ROT (1,2) = -S 

R0T(2, 1) = S 

ROT (2,2) = C 

*** rotate it 
CALL XMUL (ROT, V, N) 

END IF 

*** transform to floor coordinates 

CALL NOTHNG (FLOOR, 3) 

FL00R(1,3) = PX 
FLOOR(2,3) = PY 
CALL XMUL (FLOOR, V, N) 
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328 
r 329 
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! 331 
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*** transform to screen coordinates 
CALL XMUL (XFORM, V, N) 

*** get ready to draw, but first erase old picture 
CALL DRWFLR (VI) 

IF ((EF .EQ. 0) .AND. (EEF .NE. 0)) THEN 
CLR = 0 

CALL DRAW (VO, N, CLR) 

END IF 

CLR = 1 

CALL DRAW (V, N, CLR) 

CALL MOVE (V, VO, N) 

EEF = 1 

RETURN 

END 


~ie 

Type 

Offset P Class 



REAL*8 

594 



REAL 

4 /MG 

/ 

> 

V 

INTEGER*4 

602 


J 

REAL 

8 /MG 

/ 

3F 

INTEGER*4 

4 /ME 

/ 


INTEGERS 

0 /ME 

/ 

\G 

INTEGER*4 

0 /MG 

/ 

.OOR 

REAL 

546 


- 

REAL 

12 /MG 

/ 


INTEGER*4 

582 


1TFG 

INTEGER*4 

8 /ME 

/ 

£_ 

REAL*8 

0 * 



REAL-8 

T 


vf 

REAL 

510 



REAL 

16 /MG 

/ 


REAL*8 

586 


FORM 

REAL 

36 /MF 

/ 

I ETA 

REAL-8 

8 * 


c. 

REAL 

24 /MG 

/ 


REAL 

390 


J 

REAL 

72 /MF 

/ 

L 

REAL 

192 /MF 

/ 


REAL 

20 /MG 

/ 

ORM 

REAL 

0 /MF 

/ 
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C 


343 

C 


344 

C 


345 

C 


346 

C 


347 

C— 


348 

C 


349 

C 

— 

350 



351 

c 


352 


-1 

353 


2 

354 


i 2 

355 

100 


356 

c 
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357 
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SUBROUTINE MOVE (V, VO, N) 


This procedure saves the shape vector V 


REAL V(3, 10) , V0(3, 10) 

DO 100 K = 1, N 
DO 100 J = 1, 3 
V0(J,K) = V(J,K) 
CONTINUE 

RETURN 

END 


—Name 

Type 

J 

INTEGERS 

K 

INTEGERS 

M 

INTEGER *4 

'J 

REAL 

VO 

REAL 


Offset P Class 

614 
606 
8 * 

0 * 

4 * 
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Line# 1 
360 C 
~ 361 C 

362 

363 C 

- 364 C 

365 C— 

366 C 

367 C 

F 368 C 
1 369 C 

370 C 
r* 371 C 
372 C— 
' 373 C 

- 374 C 
I 375 

376 C 

377 
r 378 
i 379 

380 200 

- 381 

382 100 

383 C 
_ 384 

385 
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SUBROUTINE NOTHNG (A, N) 


This procedure initializes an N x N matrix A to a 
unit matrix 


REAL A(N,N) 

DO 100 K = 1, N 
DO 200 J = 1, N 
A(K, J) = 0.0 
CONTINUE 
A(K,K) = 1.0 
CONTINUE 

RETURN 

END 


ame Type 


Offset P Class 


REAL 0 * 

INTEGER* 4 626 

INTEGER* 4 618 

INTEGER*4 4 * 
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388 C 

; • 389 SUBROUTINE XMUL (R, V, N) 

390 C 
r- 391 c 

392 C— 

393 C 

r 394 C 

I 395 C This procedure uses a transformation matrix R and 

396 C transforms the shape vector V having N columns 

397 C 

r~ 398 C 

I 399 C 

400 C 
r 4oi c 



402 

REAL R(3,3) , V(3 f 10), T(3), S 


403 

INTEGER ROW, COL 


404 C 



405 

DO 100 COL = 1, N 

1 

406 

DO 200 ROW =1,3 

2 

407 

S = 0.0 

“2 

408 

DO 300 J = 1, 3 

3 

409 

S = S + R(R0W,J) * V(J, COL) 

3 

410 300 

CONTINUE 

-2 

411 

T(ROW) = S 

2 

412 200 

CONTINUE 

1 

413 

DO 400 L = 1, 3 

_2 

414 

V(L,C0L) = T(L) 

2 

415 400 

CONTINUE 

1 

416 100 

417 C 

CONTINUE 


418 

RETURN 


419 

END 

-Name Type 

Offset P Class 

COL 

INTEGER* 4 

646 


INTEGERS 

662 


INTEGERS 

666 


INTEGERS 

8 * 

R 

REAL 

0 * 

•ROW 

INTEGERS 

654 

5 

REAL 

658 

T 

REAL 

634 

y 

REAL 



■ D Line# 1 7 

387 C 
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421 C 
■**' 422 C 

: 423 SUBROUTINE ORGPOS (V, N) 

424 C 

425 C 

426 C 

• 427 C 

_ 428 C 

! 429 C This procedure calculates the shape vector V of T0M_B 

430 C at the orgin. Only the top view is considered here. 

431 C 

- 432 C 
, 433 C— 

434 C 
~ 435 C 

436 

437 

438 
'' 439 

440 C 

441 

- 442 

443 

444 C 

- 445 C 
446 

■ 447 

448 100 
“ 449 C 

450 C 

451 C 


- 452 


CL 

CC + LL 




453 

C 




Corner 

« 

A » 

454 


V(l, 1) 

= 

CC 




455 


V(2, 1) 

= 

0 




456 

C 




Corner 

« 

B » 

457 


V(l, 2) 

= 

CC 




458 


V(2, 2) 

= 

-WW 




~ 459 

C 




Corner 

« 

C » 

460 


V(l, 3) 

= 

-RR 




461 


V(2, 3) 

= 

-WW 




- 462 

C 




Corner 

« 

D » 

463 


V(l, 4) 

= 

-RR 




464 


V(2, 4) 

= 

WW 




465 

C 




Corner 

« 

E » 

466 


V(l, 5) 

= 

CC 




467 


V(2, 5) 

= 

WW 




468 

C 




Corner 

« 

MM » 

- 469 


V(l, 6) 

= 

CL 





REAL V(3, 10) , XF0RM(3,3) , V0(3,10), W(2) 

REAL Vl(3, 10) 

REAL CC, DD, LL, RR, WW, CL, SDF0RM(3,3) 

INTEGER FLAG, C0RNR(2,2), EF, EEF, PRTFG 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM, SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 


DO 100 K - 1, N 
V(3, K) = 1.0 
CONTINUE 

*** set up shape matrix V 



‘ «< 0 M V 

P L 

0 T »> 

P 

■i D Line# 1 

7 



470 


V(2, 6) 

= 0 

- 471 C 




472 


V(l, 7) 

= CL + TT 

473 


V(2, 7) 

= -DD 

- 474 C 




; 475 


V(l, 8) 

= CL - TT 

: 476 


V(2, 8) 

= -DD 

477 C 




P 478 


V(l, 9) 

= CL - TT 

479 


V(2, 9) 

= DD 

480 C 




— 481 


V(l,10) 

= CL + TT 

482 


V(2, 10) 

= DD 

483 C 




„ 484 


RETURN 


485 


END 


Name Type 


Offset 

P Class 

CC REAL 


4 

/MG / 

CL REAL 


702 


-CORNR INTEGER-4 

678 


DD REAL 


8 

/MG / 

EEF INTEGER*4 

4 

/ME / 

_EF INTEGER*4 

0 

/ME / 

FLAG INTEGER-4 

0 

/MG / 

K INTEGER*4 

694 


LL REAL 


12 

/MG / 

1 INTEGERS 

4 

* 

’RTFG INTEGER-4 

8 

/ME / 

RR REAL 


16 

/MG / 

-SDFORM REAL 


36 

/MF / 

rr REAL 


24 

/MG / 

7 REAL 


0 

* 

VO REAL 


72 

/MF / 

~1 REAL 


192 

/MF / 

./ REAL 


670 


WW REAL 


20 

/MG / 

'"FORM REAL 


0 

/MF / 


Page 16 
07-14-84 
14:03:20 

Microsoft FORTRAN 7 7 V3.13 8/05/83 
Corner << F » 


Corner « G » 


Corner « H » 
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- 488 C 

489 

490 C 

- 491 C 
492 C— 

1 493 C 

494 C 

495 C 

496 C 

497 C 

- 498 C 

499 C 

500 C— 

- 501 C 

502 C 

503 

504 

^ 505 C 

506 

507 C 

- 508 

509 

510 

511 C 

512 

513 

514 

- 515 

516 

517 200 

- 518 

519 

520 

521 C 

“ 522 

523 

524 300 

- 525 

526 

527 

. 528 C 

529 

530 

531 
" 532 

533 

534 C 

- 535 C 
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SUBROUTINE INITPL 


This procedure initializes the system and calculates 
all the necessary transformation matrices based on 
the data obtained from SIZE.DAT 


REAL V0(3, 10) ,XF0RM(3,3) ,SDFORM(3,3) , W(2) 

REAL CC, DD, LL, RR, WW, TT, Vl(3,10) 

REAL CORNR(2,2), W(2) 

INTEGER FLAG, EF, C0RNR(2,2), EEF, C0RNS(2,2), PRTFG 

COMMON /MG/ FLAG, CC, DD, LL, RR, WW, TT 
COMMON /MF/ XFORM , SDFORM, VO, VI 
COMMON /ME/ EF, EEF, PRTFG 

EEF = 0 

OPEN (7, FILE = ’SIZE.DAT’) 

READ (7, 10) CC, DD, LL, RR, WW, TT 
DO 200 K = 1, 2 

READ (7, 20) (CORNR(K, J) , J=l, 2) 

CONTINUE 
W(l) = 12.2 
W(2) = 24.4 

CALL CORDX (CORNR, XFORM, W) 

DO 300 K = 1, 2 

READ (7, 20) (C0RNS(K, J), J=l,2) 

CONTINUE 
W(l) = 12.2 
W(2) = 6.096 

CALL CORDX (CORNS, SDFORM, W) 

READ (7,20) EF 
READ (7, 20) FLAG 
READ (7, 20) PRTFG 
CLOSE (7) 

FLG = 1 

*** calculate floor shape 
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536 C 


' 

537 

JW = 30 


538 

539 C 

JL = 44 

r~ 

540 

IF (FLAG .EQ. 0) THEN 


541 

J1 = CORNR(l.l) 


542 

LI = C0RNR(1,2) 


543 

J2 = C0RNR(2, 1) 


544 

L2 = CORNR(2,2) 


545 

JJ = (L2 - LI + 1) / 2 


546 

Vl(l,l) = J1 

r~ 

547 

Vl(2,l) = LI 


548 

Vl(l ,2) = J2 


549 

Vl(2,2) = LI 

— 

550 

Vl( 1 ,3) = J2 


551 

Vl(2,3) = L2 


552 

VI ( 1,4) = J1 


553 

Vl(2,4) = L2 


554 

Vl(l, 5) = ji 


555 

Vl(2,5) - L2 + JW - JJ 


556 

Vl(l ,6) = Jl - JL 

*— 

557 

Vl(2,6) = L2 + JW - JJ 


558 

VI ( 1,7) a Jl - JL 


559 

Vl(2,7) = L2 - JL - JJ 

— 

560 

Vl(l ,8) = Jl 


561 

Vl(2,8) = L2 - JL - JJ 


562 

Vl(l ,9) = -1000.0 


563 

Vl(2,9) = -1000.0 


564 

ELSE 


565 

Jl = C0RNS(1,1) 


566 

LI = C0RNS(1 ,2) 

— 

567 

J2 = CORNS ( 2, 1) 


568 

L2 = CORNS (2,2) 


569 

V0(1,1) - Jl - JL 


570 

V0(2, 1) = L2 + 1 


571 

V0( 1,2) = J2 + JL 


572 

VO(2,2) = L2 + 1 


573 

V0( 1,3) = -1000.0 


574 

VO(2,3) = -1000.0 


575 

576 C 

END IF 

- 

577 

CALL GRAFICS 


578 

RETURN 


579 10 

FORMAT (FI 5. 8) 


580 20 

FORMAT (13) 


581 END 


ame Type 


Offset P Class 
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cc 

REAL 

4 

/MG 

/ 

; ORNR 

INTEGER-4 

714 



[ ORNS 

INTEGER*4 

730 



DD 

REAL 

8 

/MG 

/ 

r“EF 

INTEGER* 4 

4 

/ME 

/ 

1 F 

INTEGER*4 

0 

/ME 

/ 

FLAG 

INTEGER*4 

0 

/MG 

/ 

JZLG 

REAL 

758 




INTEGER*4 

750 



ul 

INTEGER*4 

770 



J2 

INTEGERS 

778 



Hr 

INTEGER*4 

786 



. L 

INTEGER*4 

766 



JW 

INTEGERS 

762 




INTEGER*4 

746 



1 

INTEGER*4 

774 



L2 

INTEGERS 

782 




REAL 

12 

/MG 

/ 

RTFG 

INTEGERS 

8 

/ME 

/ 


REAL 

16 

/MG 

/ 

SDFORM 

REAL 

36 

/MF 

/ 

~r 

REAL 

24 

/MG 

/ 

D 

REAL 

72 

/MF 

/ 

VI 

REAL 

192 

/MF 

/ 

li 

REAL 

706 



N 

REAL 

20 

/MG 

/ 

\FORM 

REAL 

0 

/MF 

/ 


582 $PAGE 




« OMV PLOT »> Page 20 

07-14-84 

r 14:03:20 

,ine// 1 7 Microsoft FORTRAN77 V3.13 8/05/83 

583 C 

- 584 C 

585 SUBROUTINE DRWFLR (V) 

586 C 
__ 587 C 

588 C 

589 C 

590 C This subroutine draws the floor portion of graphics 

591 C 

592 C 

593 c 

- 594 C 

595 C 

596 REAL V(3,10) 

_ 597 INTEGER CT, X(10), Y(10) 

598 C 

599 CT = 1 

600 C 

“*• 601 C REPEAT 

602 100 K = CT 

603 X(K) = V(1,K) 

- 604 Y(K) = V(2,K) 

605 CT = CT + 1 

606 IF (V(1,CT) .GE. -100.0) GO TO 100 

607 C UNTIL V(1,CT) < -100.0 

608 C 

609 CALL POLYGN (K, X, Y, 1) 

610 RETURN 

“ 611 END 


ame Type Offset P Class 

884 . 

888 
0 * 

804 
844 


INTEGER*4 

INTEGER*4 

REAL 

INTEGER*4 

INTEGERS 
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PLOT »> 


«< 0 M V 


V Line# 1 7 

613 C 
r 614 C 
■ 615 

616 C 

- 617 C 

: 618 C 

619 C 

620 C 

; 621 C 

622 C 

623 C 
i 624 C 

625 C 

626 C 

- 627 C 

628 C 

629 C 

630 

631 

632 

633 C 

“ 634 C 

635 C 

636 

- 637 C 

638 C 

639 C 

640 

641 

642 

643 
” 644 

645 C 

646 C 

- 647 C 

648 

649 C 

650 

651 

652 

653 C 

- 654 
655 
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SUBROUTINE DRAW (V, N, CLR) 


This procedure actually draws the top view of T0M_B. 

This procedure must be modified if different hardware 
is used for the graphics display 


REAL V(3, 10) 

INTEGER XI, X2, Yl, Y2 

INTEGER CLR 

*** draw mobile base 

CALL RCT (V, 1, CLR) 


*** draw connecting line 

XI = V( 1 , 1) 

Yl = V(2, 1) 

X2 = V(1 ,6) 

Y2 = V(2,6) 

CALL LINE (XI, Yl, X2, Y2, CLR) 

*** draw mocked-up 

CALL RCT (V, 6, CLR) 

CALL PURGE 
CALL GRFRDY 
CALL HOME 

RETURN 

END 


lame Type 


Offset P Class 


,uR INTEGERS 8 * 

I INTEGER*4 4 * 

REAL 0 * 
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< 0 M V 


Line# 1 7 

892 

INTEGERS 

r INTEGER-4 

900 

INTEGERS 

896 

INTEGER*4 

904 

l 
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657 C 

658 C 

659 

660 C 

661 C 

662 C- 

663 C 

664 C 

665 C 

666 C 

667 C 

- 668 C 

669 C- 

670 C 

_ 671 C 
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SUBROUTINE CORDX (C, T, W) 


This procedure computes' the necessary transformation 
matrices from floor to screen coordinates 



672 

INTEGER 

C(2,2) 




673 

REAL 

T(3,3) 

, W(2) 



674 C 






675 C 

set i 

up transformation matrix T 


676 C 






677 

T(l,3) = 

C(l,l) 



— 

678 

T(2,3) = 

C(2,2) 




679 

T(3,3) = 

1.0 




680 C 






681 

T(l,l) = 

(C(2, 1) 

- T(l,3)) 

/ W(l) 


682 

T(2,l) = 

(C(2,2) 

- T(2,3)) 

/ W(l) 


683 

T(3, 1) = 

0.0 




684 C 





— 

685 

T(1 ,2) = 

(C(l.l) 

- T(l,3)) 

/ W(2) 


686 

T(2,2) = 

(C( 1,2) 

- T(2,3)) 

/ W(2) 


687 

T(3,2) = 

0.0 



■“1 

688 C 






689 

RETURN 





690 

END 




~'ime Type 

Offset P Class 




INTEGER-4 

0 * 




O. 

REAL 

4 * 





REAL 

8 * 
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- 696 C 
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; 698 C 

699 

700 C 

701 C 

702 C 

703 C 
i 704 

1 705 

706 

707 

708 

709 


7 


Page 24 
07-14-84 
14:03:20 

Microsoft F0RTRAN77 V3.13 8/05/83 


This is a graphics package for the TECMAR GRAPHICS MASTER board 
written under Microsoft's FORTRAN 77. To use this package, one 
must include this package in the source file. A graphics master 
must already be installed, or the software will hang. 


SUBROUTINE PURGE 

This procedure purges the graphics buffer and forces the board 
to complete the drawing by closing the graphics channel. 

INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
CLOSE (GRF) 

RETURN 

END 


ne Type 


Offset P Class 


or 

F 


CHAR-1 

INTEGER-4 


4 /GMBD / 

0 /GMBD / 
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713 
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716 C 

717 C 

718 C 

719 

720 

721 

722 

723 
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SUBROUTINE GRFRDY 

This procedure opens the graphics channel and sets it ready for 
communication 


INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
OPEN (GRF, FILE = 'gm') 
RETURN 
END 


Name 

Type 

Offset P Class 


ESC 

CHAR*1 

4 

/GMBD 

/ 

GRF 

INTEGERS 

0 

/GMBD 

/ 


725 $PAGE 



I iine# 1 
726 C 

- 727 C 

728 

729 C 
_ 730 C 
i 731 C 

732 C 

733 C 
r- 734 C 

735 

736 

- 737 

738 

739 

740 10 

741 
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SUBROUTINE SETFB (FG, BG) 


This procedure sets the foreground color to FG and the backgroun 
color to BG. Both arguments must be of INTEGER type. 


INTEGER GRF, FG, BG 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
WRITE (GRF, 10) ESC, FG, BG 
RETURN 

FORMAT (' ', Al, 12, 12, 'c'\) 

END 


ame Type 

INTEGERS 
SC CHAR-1 
a INTEGER*4 

F INTEGERS 


Offset P Class 
4 - 

4 /GMBD / 

0 * 

0 /GMBD / 
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JD I 

,ine# 

1 


743 

C 


744 

C 


745 


— • 

746 

C 

; 

747 

C 


748 

C 


749 

C 

- 

750 

C 

i . 

751 

C 


752 


1 

753 


i 

l 

754 



755 

C 

n 

756 


| 

757 



758 



759 


1 

760 


i . 

761 

C 


762 



763 



764 



765 

C 


766 

10 


767 

20 


768 

30 


769 



SUBROUTINE GRAFICS 

This procedure enters the GM graphics mode with a four-line text 
window at the bottom 


INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 

GRF = 9 
ESC = CHAR(27) 

CALL GRFRDY 

WRITE (GRF, 10) ESC 
WRITE (GRF, 20) ESC 
WRITE (GRF, 30) ESC 
CALL SETFB (1,0) 

CALL HOME 
RETURN 

FORMAT (’ \ Al, 1 [ !0m'\) 

FORMAT (' ', Al, * [ !640;352;2g’\) 
FORMAT (' ', Al, , [21;24r , \) 

END 


Name Type 


Offset P Class 


•CHAR 

ESC CHAR*1 

GRF INTEGER*4 


INTRINSIC 
4 /GMBD / 

0 /GMBD / 
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~ 789 

790 

791 10 

- 792 30 
793 


PLOT »> Page 28 

07-14-84 

14:03:20 

7 Microsoft F0RTRAN77 V3.13 8/05/83 


SUBROUTINE QUITGM 


This procedure gets one out of graphics mode and returns 
to text mode 


CHARACTER CH, ESC 

INTEGER GRF 

COMMON /GMBD/ GRF, ESC 

CALL HOME 
WRITE (GRF, 30) 

CALL PURGE 
READ (*, 10) CH 
CALL GRFRDY 
CALL TEXT 
RETURN 
FORMAT (Al) 

FORMAT ( f Press <CR> to continue ... '\) 
END 


me Type 


Offset P Class 


CHAR-1 
C CHAR*1 

INTEGER-4 


1015 

4 /GMBD / 

0 /GMBD / 
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807 
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TEXT 


This procedure returns the system to text mode 


INTEGER GRF 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
WRITE (GRF, 10) ESC 
RETURN 



809 10 

810 

FORMAT (' 
END 

', Al, ' [ !80;25; la’\) 

_Name 

* Type 

Offset 

P Class 

ESC 

CHAR*1 

4 

/GMBD / 

GRF 

INTEGER-4 

0 

/GMBD / 
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uine# 1 
812 C 
"" 813 C 
. . 814 
815 C 

r 816 c 

I 817 C 
818 C 
_ 819 C 
820 
821 
822 
~ 823 

824 10 

825 
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SUBROUTINE LINE (XI, Yl, X2, Y2, COLOR) 

This procedure draws a line from (XI, Yl) to (X2,Y2) in COLOR 


INTEGER GRF, XI, Yl, X2, Y2, COLOR 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 

WRITE (GRF, 10) ESC, XI, Yl, X2, Y2, COLOR 

FORMAT (' ', Al, '[!’, 4(13,’;’), 13, *1*\) 

END 


le 

Type 

Offset P 

Class 

3L0R 

INTEGER-4 

16 * 


i 

j 

CHAR-1 

4 

/GMBD 

? 

INTEGER-4 

0 

/GMBD 

1 

INTEGER *4 

0 * 



INTEGER-4 

8 * 



INTEGER-4 

4 * 


2 

INTEGER-4 

12 * 
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SUBROUTINE HIDELN (XI, Yl, X2, Y2, COLOR) 


This procedure draws the line (X1,Y1) - (X2,Y2) but aborts drawi 
before reaching target if a dot in a color other that BG is 
encountered 


INTEGER 

GRF, 

XI, 

Yl, 

X2, 

Y2, 

COLOR 

CHARACTER 

ESC 






COMMON 

/GMBD/ GRF, 

ESC 





WRITE (GRF 

, 10) ESC, 

XI, 

Yl, 

X2, 

Y2, 

COLOR 


RETURN 

FORMAT (' Al, '[!', 4(13, 13, 'S’\) 

END 


ime 

Type 

Offset 

P Class 

COLOR 

INTEGERS 

16 

•A. 

'r* 


3C 

CHAR-1 

4 


/GMBD 

oRF 

INTEGER-4 

0 


/GMBD 

XI 

INTEGERS 

0 

* 


2 

INTEGERS 

8 

n* 


1 

INTEGER-4 

4 

'f* 


Y2 

INTEGER-4 

12 




844 $PAGE 




C‘n 
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in e// 
845 


846 

847 

848 

849 

850 

851 

852 

853 

854 

855 

856 

857 

858 


1 

C 

C 

c 

c 

c 

c 

c 

c 


__ 859 
' 860 100 
861 
862 

- 863 10 

864 20 

865 30 

- 866 
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SUBROUTINE POLYGN (N, X, Y, COLOR) 


This procedure draws a closed polygon whose N vertices are store 
in the arrays X and Y. The color to be used is COLOR 


INTEGER GRF, X(N), Y(N) , COLOR 

CHARACTER ESC 

COMMON /GMBD/ GRF, ESC 
WRITE (GRF, 10) ESC 
DO 100 K = 1, N 

WRITE (GRF, 20) X(K), Y(K) 

CONTINUE 

WRITE (GRF, 30) COLOR 
RETURN 

FORMAT (' \ Al, ' [ ! * \) 

FORMAT ( 2(13, ' ; * )\) 

FORMAT ( '13, V\) 

END 


ame Type 

~L0R INTEGER-4 
„C CIIAR-1 

RF INTEGERS 

INTEGERS 
INTEGER*4 
INTEGERS 
INTEGER-4 


Offset P 

Class 


12 * 



4 

/GMBD 

/ 

0 

/GMBD 

/ 

1160 



0 * 



4 - 



8 * 
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r 868 C 

; 869 c 

870 SUBROUTINE HOME 

— 871 C 

! 872 C 

873 C THIS SUBROUTINE HOMES THE CURSOR ■ 

_ 874 C 

I 875 C 

i 876 INTEGER GRF 

877 CHARACTER ESC 

r 878 C 

I 879 COMMON /GMBD/ GRF, ESC 

880 C 

- 881 WRITE (GRF, 10) ESC 

882 RETURN 

883 10 FORMAT (' ', Al, ’[ ljl f'\) 

_ 884 END 

Name Type Offset P Class 

-ESC CHAR*1 4 /GMBD / 

GRF INTEGER*4 0 /GMBD / 


885 $PAGE 



< 0 M V P L 0 T »> 


Li.n e# 1 


» Type 

Size 

Class 

IRDX 


SUBROUTINE 

:-“7 


SUBROUTINE 

: ? LR 


SUBROUTINE 

lliD 

5 

COMMON 

£FIC 


SUBROUTINE 

RDY 


SUBROUTINE 

'~ELN 


SUBROUTINE 

)ME 


SUBROUTINE 

"“TPL 


SUBROUTINE 

E 


SUBROUTINE 

■» 

j 

12 

COMMON 

1 

312 

COMMON 


28 

COMMON 

JV E 


SUBROUTINE 

)THNG 


SUBROUTINE 

IPOS 


SUBROUTINE 

-JT 


SUBROUTINE 

ILYGN 


SUBROUTINE 

"1GE 


SUBROUTINE 

:tgm 


SUBROUTINE 

nrp 


SUBROUTINE 

DRAW 


SUBROUTINE 

FFB 


SUBROUTINE 

J.DEVE 


SUBROUTINE 

INCOS 


SUBROUTINE 

XT 


SUBROUTINE 

PVEW 


SUBROUTINE 

MUL 


SUBROUTINE 


No Errors Detected 
885 Source Lines 
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Pass One 


Appendix 4 


OMV — Data files 



File : INITCON.DAT 


0.0 

0.0 

0.0 

0.00 

0.0 

0.0 

0.0 

0.0 

0.0 


This file contains all the needed initial conditions 


P0S(1) — initial condition 

P0S(2) — initial condition 

P0S(3) — initial condition 

VEL(l) — initial condition 

VEL(2) — initial condition 

VEL(3) — initial condition 

EUL(l) — initial condition .. ROLL 

EUL(2) — initial condition .. PITCH 

EUL(3) — initial condition .. YAW 




File : 


MDLPRM.DAT 


This file contains all the model parameters needed by OMV 


00.075 

00.075 

00.075 

000.52359878 

000.52359878 

000.52359878 

7048.37 

3713.95 

3713.95 

3282.75 

0.1 

1 

10 

200.0 


ACC(l) : Acc along X-axis (body) 

ACC(2) : Acc along Y-axis (body) 

ACC(3) : Acc along Z-axis (body) 

WWB(l) : body rate about X axis 

WWB(2) : body rate about Y axis 

WWB(3) : body rate about Z axis 

III(l) principal moment of inertia along 1 axis 

111(2) principal moment of inertia along 2 axis 

111(3) principal moment of inertia along 3 axis 

Mass in kilograms 

major cycle period in seconds 

MODE : 1 for position control 

No. of steps per major cycle 

altitude of orbit in kilo-meters 




File : SVXINT.DAT 

This file contains all the system initialization data needed by the SVX module 


0.5588 

CC 

IN METERS 

0.762 

LL 

IN METERS 

11.668 

AA 

IN METERS 

2.4384 

HH 

IN METERS 

7048.37 

IINV(l) 

3713.95 

IINV(2) 

3713.95 

IINV(3) 




File 


HNDSGL.DAT 


This file contains the simulated hand controller signals 

(Partial list) 


100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 

100000000000 



File 


SIZE.DAT 


This file contains all the plot parameters 
for the 

graphics package PLOT 


0.5588 

2.1336 

0.762 

1.016 

0.6096 

0.3048 

409 

001 

630 

350 

100 

152 

500 

300 

000 

000 

001 


CC : 22 inches 

DD : 84 inches 

LL : 30 inches 

RR : 40 inches 

WW : 24 inches 

TT : 12 inches 

C0RNR(1,1) 

C0RNR(1 ,2) 

C0RNR(2, 1) 

C0RNR(2,2) 

CORNR(l.l) SIDE VIEW 

C0RNR(1,2) SIDE VIEW 

C0RNR(2, 1) SIDE VIEW 

C0RNR(2,2) SIDE VIEW 

PLOT MODE : <> 0 MEANS NO CLEAR 

VIEW : 0 = TOP VIEW, <> 0 = SIDE VIEW 

PRTFG: 1-PLOT 2-PRINT 3-PLOT & PRINT 
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- 07-14-84 

12:51:14 

Line# 1 7 Microsoft FORTRAN77 V3.13 8/05/83 

- 1 $LINESIZE:79 

2 $PAGESIZE: 56 

3 $TITLE: *<« 0 M V >»' 

4 C 

i 5 C OMV SIMULATION MODEL 

! 6 c 

7 C 

~ 8 C by 

9 C 
10 C 

_ 11 C Dr. W. Teoh 

12 C 

13 C UAH Huntsville 

14 C 1984 

15 C 

16 C 

17 C 

—■ 18 C This is a simplified version of a mathematical simulation 

19 C model of the OMV. In this model, the following simplfications 

20 C and assumptions are made : 

_ 21 C 

22 C 1. The hand controllers provide signals that are interpreted 

23 C as a force at the center of mass and/or a torque about the 

24 C center of mass to provide a rotation of constant angular 

~ 25 C velocity. 

26 C 2. The target vehicle is in a circular orbit; the altitude of 

27 C this orbit is inputted from the MDLPRM.DAT file. 

- 28 C 3. Orbital mechanics is implemented, but smaller perturbation 

29 C effects are totally ignored. 

30 C 4. Detailed placement of thrusters is not considered (Please 

31 C see assumption 1. above) 

32 C 5. Roll, pitch and yaw denote the instantaneous orientation 

33 C of the OMV. 

34 C 

^ 35 C A 14 component state vector is generated by this model, and 

36 C this state vector serves as input to the SVX module. 

37 C 

38 C 

39 c 

40 C 

41 REAL * 8 X(3) , V(3), E(3), A(3), W(3), Q(4) 

42 REAL* 8 P0S(3), VEL(3), EUL(3), OMEGA 

43 REAL * 8 111(3), S(14), MASS, CYCLE 

44 INTEGER CMD(7), IN, FLAG, MODE, STEP 

45 INTEGER * 4 TIME 

46 C 

47 COMMON /MC/ III, MASS, CYCLE, MODE, STEP 

48 COMMON /PC/ P0S, VEL, EUL, OMEGA 

49 C 
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D Line# 1 

7 Microsoft FORTRAN77 

V3.13 8/05/83 

— * 

50 C 

*** system initialization 



51 C 




52 

IN = 2 


r _ 

53 

TIME = -1 


j 

54 

CALL OMVMDL (IN) 


1 

55 

OPEN (IN, FILE = 'HNDSGL.DAT') 



56 C 




57 C 

*** *** Note : this invokes graphics routines, and can be 


58 C 

elimiated if no graphics output. 



59 C 



— 

60 

CALL INITPL 



61 C 




62 C 

*** calculate the initial quaternions at the start of the 

M, 

63 C 

*** simulation and read hand controller 



64 C 




65 

CALL DETQ (EUL, Q) 



66 

CALL HNDCTL (IN, FLAG, A, W) 



67 

CALL MATCH (EUL, POS, VEL, E, X, V, 3) 



68 

CALL STATE (Q, S, W) 



69 

CALL SVX (S, CMD, MODE) 


— 

70 

CALL OUTPUT (A, W, X, V, E, Q, S, CMD, TIME) 



71 

TIME = 0 



72 C 



_ 

73 C 

*** main processing loop 



74 C 




75 C 

WHILE (FLAG = 0) DO 



76 100 

IF (FLAG .NE. 0) GOTO 900 



77 C 




78 C 

*** copy initial state into. work vectors and 

use these 


79 C 

*** work vectors for solving the equations of 

motion 

- 

80 C 




81 

CALL MOTION (X, V, E, A, W, Q) 



82 C 




83 C 

*** update dynamic state 



84 C 




85 

CALL MATCH (E, X, V, EUL, POS, VEL, 3) 



86 C 



- 

87 C 

*** calculate state vector and pass it on to 

the State 


88 C 

*** Vector Transformation module 



89 C 




90 

CALL STATE (Q, S, W) 



91 

CALL SVX (S, CMD, MODE) 



92 

CALL OUTPUT (A, W, X, V, E, Q, S, CMD, TIME) 



93 C 




94 C 

*** poll hand controller and get the next set 

of signals 


95 C 




96 

CALL HNDCTL (IN, FLAG, A, W) 


■ 

97 

GOTO 100 



98 C 

END WHILE 




:< o m v 


Line# 1 
r~ 99 900 
100 

1 101 c 

_ 102 C 
I 103 C 
1 104 

105 C 
~ 106 C 

107 C 

108 
- 109 
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CONTINUE 
CLOSE (IN) 

*** *** This is also a call to the graphics package 
CALL QUITGM 

*** Grand exit, stage left 

STOP 

END 


ame 

Type 

Offset 

P Class 


— 

REAL* 8 

242 



J 

INTEGER*4 

266 



KCLE 

REAL*8 

32 

/MC 

/ 


REAL* 8 

74 



j 

REAL* 8 

48 

/PC 

/ 

LAG 

INTEGER*4 

302 



LI 

REAL*8 

0 

/MC 

7 


INTEGER*4 

294 



rtSS 

REAL*8 

24 

/MC 

/ 

ODE 

INTEGER*4 

40 

/MC 

/ 

"'EGA 

REAL*8 

72 

/PC 

/ 

S 

REAL* 8 

0 

/PC 

/ 


REAL*8 

98 



— 

REAL*8 

130 



EP 

INTEGER*4 

44 

/MC 

/ 

IME 

INTEGER*4 

298 




REAL*8 

26 



,L 

REAL*8 

24 

/PC 

/ 


REAL*8 

50 




REAL*8 

2 
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- Ill C 

112 C 

1 113 SUBROUTINE OMVMDL (IN) 

_ 114 C 

I 115 C 

i 116 C 

117 C This procedure obtains the necessary parameters of the OMV 

118 C by reading them from a disk file called MDLPRM.DAT after 

119 C getting the initial state of the OMV (from a file called 

120 C INITCON.DAT 

- 121 C 

122 C 

123 C 

124 REAL * 8 P0S(3), VEL(3), EUL(3), OMEGA 

125 REAL * 8 ACC(3), 111(3), WWB(3), INV(3) 

126 REAL * 8 MASS, CYCLE, ORBIT 

127 INTEGER IN, MODE, STEP 

128 C 

129 COMMON /DC/ ACC, WWB 

130 COMMON /MC/ III, MASS, CYCLE, MODE, STEP 

131 COMMON /PC/ POS, VEL, EUL, OMEGA 

132 C 

133 C *** get initial conditions of the OMV 

134 C 

135 OPEN (IN, FILE = ’INITCON.DAT’) 

136 CALL VECTOR (IN, POS, 3) 

137 CALL VECTOR (IN, VEL, 3) 

138 CALL VECTOR (IN, EUL, 3) 

139 CLOSE (IN) 

140 C 

141 C *** read acceleration, angular rates and 

142 C *** principal moments of inertia in body frame 

143 C 

144 OPEN (IN, FILE = ’MDLPRM.DAT’) 

145 CALL VECTOR (IN, ACC, 3) 

146 CALL VECTOR (IN, WWB, 3) 

147 CALL VECTOR (IN, III, 3) 

148 C 

149 C *** read mass characteristics & other parameters 

150 C 

151 READ (IN, 10) MASS 

152 READ (IN, 10) CYCLE 

153 READ (IN, 20) MODE 

154 READ (IN, 30) STEP 

155 READ (IN, 10) ORBIT 

156 CLOSE (IN) 

157 C 

158 C 

159 C 


*** calculate orbital frequency 



« 0 M V »> 


Line# 1 
- 160 
1 161 C 

1 162 C 

163 

r 164 10 

165 20 

166 30 
- 167 


CALL ANGFRE (ORBIT, OMEGA) 


RETURN 

FORMAT (F15.8) 
FORMAT (II) 
FORMAT (12) 

END 
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ame 

Type 

j 

REAL*8 

.CLE 

REAL*8 

QL 

REAL*8 

"T 

REAL*8 

INTEGER*4 

NV 

REAL*8 

«5S 

REAL*8 

IE 

INTEGER*4 

n£GA 

REAL*8 

RBIT 

REAL*8 


REAL*8 

*£P 

INTEGER*4 

EL 

REAL* 8 

"B 

REAL*8 


Offset P Class 


0 

/DC 

/ 

32 

/MC 

/ 

48 

/PC 

/ 

0 

/MC 

/ 

0 * 
306 

24 

/MC 

/ 

40 

/MC 

/ 

72 

/PC 

/ 

330 

0 

/PC 

/ 

44 

/MC 

/ 

24 

/PC 

/ 

24 

/DC 

/ 
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D Line# 

_ 169 

! 170 

1 171 

172 

r 173 

174 

175 

176 

177 

178 

179 

180 
181 
182 

183 

184 

185 

186 

187 

188 

189 

190 

191 
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C 

C 

SUBROUTINE ANGFRE(0RB, W) 

C 

C 

C 

C 

C This procedure calculates the orbital angular frequency 

C at a given altitude. In this calculation, the altitude 

C must be given in kilo-meters. This is necessary in order 

C for the calculations to be carried out without lossing 

C precision. The angular frequency W is in rad/second 

C 

C 

C 

REAL * 8 ORB 
REAL * 8 ALT, R3, W 
C 

ALT = ORB * 0.001 

R3 = (6.370 + ALT) ** 3 

W = DSQRT (398.86 / R3) * 0.001 

RETURN 

END 


Name Type Offset P Class 


ALT 

REAL*8 

■DSQRT 


ORB 

REAL*8 

R3 

REAL*8 

W 

REAL*8 


358 

INTRINSIC 

0 * 

366 
4 * 


192 $PAGE 
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.Line# 1 
! 193 C 

1 194 C 

195 

" 196 C 

197 C 

198 C— 
- 199 C 

200 C 

201 C 

202 C 

203 C 

204 C— 

205 C 
“ 206 

207 

208 C 

209 

210 

211 100 
212 

213 10 

214 
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SUBROUTINE VECTOR (M, A, N) 


This procedure reads a vector A of N elements from input 
unit M 


INTEGER M, N 

REAL * 8 A(N) 

DO 100 K = 1, N 

READ (M, 10) A(K) 
CONTINUE 
RETURN 

FORMAT (F15.8) 

END 


~ne Type 


Offset P Class 


REAL*8 4 * 

INTEGERS 374 

INTEGERS 0 * 

INTEGER*4 8 * 


215 $PAGE 
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216 C 

217 C 

218 SUBROUTINE HNDCTL (IN, FLAG, A, W) 

219 C 

220 C 

221 C 

222 C Simulates hand controllers input by reading from a file 

223 C (called HNDSGL.DAT 12) integers to simulate a 12 bit output 

224 C of the hand controllers. Bit assignments are as follows : 


225 

C 




226 

C 

bit 

meaning (direction in 

body frame) 

227 

C 

=== 

— 


228 

C 

1 

Accelerate along +1 

axis 

229 

C 

2 

Accelerate along -1 

axis 

230 

C 

3 

Accelerate along +2 

axis 

231 

C 

4 

Accelerate along -2 

axis 

232 

C 

5 

Accelerate along +3 

axis 

233 

C 

6 

Accelerate along -3 

axis 

234 

C 

7 

Rotate about +1 

axis 

235 

C 

8 

Rotate about -1 

axis 

236 

C 

9 

Rotate about +2 

axis 

237 

C 

10 

Rotate about -2 

axis 

238 

C 

11 

Rotate about +3 

axis 

239 

C 

12 

Rotate about -3 

axis 

240 

C 




241 

C 




242 

C 




- 243 


REAL * 8 

ACC(3) , WWB(3) 


244 


REAL * 8 

A(3) , W(3) 


245 


INTEGER 

SL(6) , SA(6) , FLAG 


_ 246 


COMMON /DC/ 

ACC, WWB 


247 

c 




248 


FLAG = 0 



249 


READ (IN, 10, : 

END = 90, ERR = 90) SL, 

SA 

~ 250 

c 




251 

c 

*** no error, 

generate matrices A and 

[ W 

252 

c 




- 253 


CALL FUDGE (A 

, ACC, SL) 


254 


CALL FUDGE (W 

, WWB, SA) 


255 


RETURN 



256 

90 

CONTINUE 



257 

C 




258 

C 

*** error condition 


259 

C 




~ 260 


FLAG = 1 



261 


RETURN 



262 

10 

FORMAT (2011) 




- 263 END 


0 M V »> 


Line# 1 7 

r— 

e Type 

- REAL*8 
REAL* 8 

,aG INTEGER*4 
L INTEGER*4 

INTEGER*4 
INTEGER*4 
REAL*8 
*1 REAL*8 


Offset P Class 
8 * 

0 /DC / 

4 * 

0 * 

414 
390 
12 * 

24 /DC / 
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264 $PAGE 


«< o 


D Line# 
- 265 

266 

267 

268 

269 

270 

271 

272 

273 

274 

275 

276 

277 
_1 278 

1 279 

1 280 
1 281 
"1 282 
1 283 

1 284 

-1 285 

1 286 

287 

288 
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C 

C 

SUBROUTINE FUDGE (A, ACC, SL) 

C 

c ' 

c 

C *** Sets appropriate components based on SL 

C 

C 

c 

INTEGER SL(6) , T, K, J 

REAL * 8 ACC(3) , A(3), X 
DO 100 K = 1, 6, 2 

J = (K + 1) / 2 

X = 0.0 

T = SL(K) + SL(K+1) 

IF (T .EQ. 1) THEN 

X = ACC(J) 

IF (SL(K) .EQ. 0) X = -X 
END IF 
A(J) = X 

100 CONTINUE 
RETURN 
END 


Name 

Type 

Offset 

"A 

REAL*8 

0 

ACC 

REAL*8 

4 

J 

INTEGER*4 

450 

-K 

INTEGER*4 

446 

SL 

INTEGERS 

8 

T 

INTEGERS 

462 

X 

REAL*8 

454 


289 $PAGE 



n 


« 0 M V »> 


Line# 1 


290 

291 

292 

293 

294 

295 

296 

297 
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SUBROUTINE STATE (Q, S, W) 


This procedure uses the dynamic quantities of the OMV and 
constructs a State Vector of the OMV. The 14 components of 


298 C 

299 C 

300 C 
mi r 

this State Vector S are defined as follows 
Components Meaning 



JOl V-* 

302 C 

S(l) .. S(3) 

Relative displacement between 

OMV 

and 

303 C 


target 



304 C 

S(4) .. S(6) 

Relative velocity between OMV ■ 

& target 

305 C 

S( 7) .. S(9) 

Angular momentum vector of OMV 

in 

LVF 

306 C 

S(10) .. S(13) 

Attitude quaternions expressed 

in 

body 

307 C 


frame, and 



308 C 

S( 14) 

Instantaneous mass, assumed < 

constant 

309 C 


throughout the simulation. 



310 C 





311 C 





312 C 

— 




— 

313 C 





314 

REAL * 8 

P0S(3) , VEL(3) , EUL(3) , OMEGA 



315 

REAL * 8 

111(3), QQ(4) , MASS, CYCLE 



316 

REAL * 8 

LB(3) , LL(3) , B(3,3) , A(3,3) 



317 

REAL * 8 

Q(4), W(3), L(3) , S(14) 



318 

INTEGER 

MODE, STEP 



319 C 





320 

COMMON /MC/ 

III, MASS, CYCLE, MODE, STEP 



321 

COMMON /PC/ 

POS, VEL, EUL, OMEGA 



322 C 





323 C 

*** calculate angular momentum in body frame 



324 C 





325 

CALL DOTPRD (III, W, LB, 3) 



326 C 





327 C 

*** transforms it to local vertical frame 



328 C 





329 

CALL DCSINV (Q, 

B) 



330 

CALL OMUL (B, 

LB, LL, 3) 



331 C 


• 



332 C 

*** Build state 

vector 



333 C 





334 

N = 0 




335 

CALL PUT (N, S, 

POS, 3) 



336 

CALL PUT (N, S, 

VEL, 3) 



337 

CALL PUT (N, S, 

LL , 3) 



338 

CALL PUT (N, S, 

Q , 4) 





MASS 
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340 C 

341 RETURN 

- 342 END 
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Name 

Type 

Offset P Class 


i 

REAL*8 

642 


3 

REAL*8 

570 


CYCLE 

REAL*8 

32 /MC 

/ 

~:ul 

REAL*8 

48 /PC 

/ 

:n 

REAL*8 

0 /MC 

/ 

L 

REAL*8 

546 


-LB 

REAL*8 

498 


,L 

REAL*8 

522 


MASS 

REAL*8 

24 /MC 

/ 

HODE 

INTEGER*4 

40 /MC 

/ 


INTEGER*4 

714 


JMEGA 

REALMS 

72 /PC 

/ 

POS . 

REAL*8 

0 /PC 

/ 

n 

REAL*8 

0 * 


!Q 

REAL*8 

466 


S 

REAL*8 

4 * 


-STEP 

INTEGERS 

44 /MC 

/ 

EL 

REAL*8 

24 /PC 

/ 

W 

REAL*8 

8 * 


343 $PAGE 
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Line# 
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: 345 

346 

- 347 

348 

349 
__ 350 

351 

352 

353 
~ 354 

355 

356 

- 357 

358 

359 

360 

361 

362 

363 
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C 

C 

SUBROUTINE PUT (N, S, A, M) 

C 

C 

c 

C The procedure copies a vector A into a larger one S 

C starting at the N-th element of S 

C 

C : 

C 

REAL * 8 S( 14) 

REAL * 8 A(M) 

C 

DO 100 K = 1, M 
N = N + 1 
S(N)= A(K) 

100 CONTINUE 
RETURN 
END 


me Type 

REAL*8 

INTEGER*4 

INTEGER*4 

INTEGER*4 

REAL*8 


Offset P Class 

8 * 

718 
12 * 

0 * 

4 * 


364 $PAGE 


z « o a > 


«< 0 M V »> 


Page 14 
07-14-84 
12:51:14 

D Line# 1 7 Microsoft F0RTRAN77 V3. 13 8/05/83 

r 365 C 

366 C 

367 SUBROUTINE DOTPRD (A, B, C, N) 

- 368 C 

369 C 

370 C 

_ 371 C *** This procedure calculates a vector C from two other 

372 C vectors A and B such that 

373 C C(I) = A(I) * B(I) 

374 C for all i = 1 to N 

375 C 

376 C 

377 C 

378 REAL * 8 

379 DO 100 K 

1 380 C(K) 

1 381 100 CONTINUE 

382 RETURN 

383 END 

"Name Type Offset P Class 

REAL*8 0 * 

REAL*8 4 * 

REAL*8 8 * 

INTEGER-4 726 

INTEGER*4 12 * 


384 $PAGE 


A(N), B(N) , C(N) 
= 1. N 

= A(K) * B(K) 



;« o H v 


) Line# 1 

- 385 C 

386 C 

387 

_ 388 C 

389 C- 

390 C 

391 C 

~ 392 C 

393 C 

394 C- 

- 395 C 

396 

397 

398 C 

399 

400 

401 

- 402 

403 

404 

_ 405 C 

406 

407 

408 

409 

410 C 

411 

- 412 
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SUBROUTINE DETQ (E, Q) 


*** calculates quaternions from the Euler angles 
using expression given by Zack. 


REAL * 8 E(3), Q(4) 

REAL * 8 Cl, SI, C2, S2, C3, S3, THETA 

THETA = E( 1) / 2.0 

CALL SINC0S (THETA, SI, Cl) 

THETA = E(2) / 2.0 

CALL SINCOS (THETA, S2, C2) 

THETA = E(3) / 2.0 

CALL SINCOS (THETA, S3, C3) 

Q(l) = SI * C3 * C2 + Cl * S3 * S2 

Q(2) = SI * S3 * C2 + Cl * C3 * S2 

Q(3) = Cl * S3 * C2 - SI * C3 * S2 

Q(4) = Cl * C3 * C2 - SI * S3 * S2 

RETURN 

END 


lame 

Type 

Offset 

P Class 

L 

REAL*8 

750 


_ 2 

REAL*8 

766 


:3 

REAL*8 

782 


■ 

REAL*8 

0 

* 


REAL*8 

4 

* 

51 

REAL*8 

742 


is 

REAL*8 

758 


3 

REAL*8 

774 


HETA 

REAL* 8 

734 
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r- 414 C 

415 C 

416 SUBROUTINE SINCOS (THETA, S, C) 

- 417 C 

I 418 C 

419 C 

_ 420 C 

421 C *** this procedure returns the sine and cosine of an 

422 C angle THETA. 

423 C 

- 424 C 

425 C 

426 REAL * 8 THETA, S, C, A 

427 C 

! 428 C = DCOS (THETA) 

‘429 S = DSIN(THETA) 

430 RETURN 

431 END 

Name Type Offset P Class 

8 * 

INTRINSIC 
INTRINSIC 

4 # 

0 * 


432 $PAGE 


REAL*8 
REAL-8 

DCOS 

DSIN 

S REAL*8 

THETA REAL*8 



& OHV »> 


i Line# 
~ 433 

434 

435 
_ 436 
i 437 
: 438 

439 

440 

441 

442 
p 443 
I 444 
‘ 445 
r- 446 

447 
1 448 

449 
~ 450 

451 

452 

- 453 

454 

455 

456 

457 

458 

459 
“ 460 

461 

462 

- 463 
464 
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470 

471 

472 

473 

474 

475 

476 

477 

478 

479 

480 

481 


SUBROUTINE MOTION (X, V, E, A, W, Q) 


*** This procedure solves the equation of motion 


REAL * 8 
REAL * 8 
REAL * 8 
REAL * 8 
REAL * 8 
INTEGER 

COMMON 
COMMON 


/MC/ 

/PC/ 


P0S(3), VEL(3) , EUL(3) , OMEGA 

X(3) , V(3), E(3), A(3), W(3), Q(4) 

CIN(3,3) , C(3,3), AA(3, 10) , B(3), QQ(4) 

VIW(3) t PI, TWO 

111(3) , MASS, CYCLE 

MODE, STEP 

III, MASS, CYCLE, MODE, STEP 
POS, VEL, EUL, OMEGA 


H = CYCLE / FLOAT (STEP) 

N = STEP 

PI = 355.0 / 113.0 
TWO= PI * 2.0 

*** Divide 1 major cycle into N equal subintervals and 
*** determine the OMV state for each interval 

DO 100 KK = 1, N 

*** Update orientation 


465 

DO 200 J = 1, 3 

466 

WW(J) = W(J) * H 

467 

E(J) = E(J) + WW(J) 

468 - 

IF (E(J) .GT. TWO) 

469 200 

CONTINUE 


*** Calculate quaternion for this rotation, and transform 
*** it to local vertical frame with respect to initial frame 

CALL DETQ( WW, QQ) 

CALL UPDQ (Q, QQ) 

*** from the direction cosine matrix, calculate the 
*** acceleration vector in LVF and store it in the 
*** acceleration matrix AA 

CALL DCSINV (Q, CIN) 
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D Line# 1 

7 Microsoft 

— 1 

482 

CALL DMUL (CIN, A, B, 3) 

1 1 

483 

CALL STORE (B, AA, KK) 

‘ 1 

484 100 

CONTINUE 


485 C 


| 

486 C 

*** Solve the equation of motion using 

* 

487 C 

488 C 

*** method 

1 

489 

490 C 

CALL SOLVE (X, V, AA, N, H, OMEGA) 


491 

RETURN 

r 

492 

END 


Name 

Type 

Offset P Class 


\ 

REAL-8 

12 * 


AA 

REAL*8 

990 


B 

REAL-8 

1230 


! 

REAL*8 

918 


:in 

REALMS 

846 


CYCLE 

REAL* 8 

32 /MC 

/ 

_-J7 

REAL*8 

8 * 


2UL 

REAL*8 

48 /PC 

/ 

FLOAT 


INTRINSIC 

_H 

REAL 

1254 


III 

REAL*8 

0 /MC 

/ 

J 

INTEGER*4 

1286 


KK 

INTEGER*4 

1278 


““MASS 

REAL*8 

24 /MC 

/ 

MODE 

INTEGER*4 

40 /MC 

/ 

N 

INTEGER*4 

1258 


-OMEGA 

REAL*8 

72 /PC 

/ 

PI 

REAL*8 

1262 


POS 

REAL*8 

0 /PC 

/ 

_Q 

REAL* 8 

20 * 


)Q 

REAL*8 

814 


STEP 

INTEGERS 

44 /MC 

/ 

TWO 

REAL*8 

1270 


-*/ 

REAL*8 

4 * 


/EL 

REAL*8 

24 /PC 

/ 

W 

REAL* 8 

16 * 


_WW 

REAL*8 

790 


< 

REAL* 8 

0 * 



493 $PAGE 



<< 0 M V 


i Line/' 1 
- 494 C 
495 C 
1 496 

__ 497 C 

498 C 

499 C— 
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^ 507 
508 
' 509 

510 
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512 
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SUBROUTINE MATCH (A, B, C, P, Q, R, N) 


*** This procedure makes an exact duplicate B of a 
vector A of N elements 


REAL * 8 A(N) , B(N) , C(N), P(N), Q(N), R(N) 
DO 100 K = 1, 3 
P(K) = A(K) 

Q(K) = B(K) 

R(K) = C(K) 

CONTINUE 

RETURN 

END 


me 

Type 

Offset 

P Class 


REAL*8 

0 

* 


REAL*8 

4 

* 


REAL*8 

8 

* 

f 

INTEGER*4 

1290 



INTEGER*4 

24 

* 


REAL*8 

12 

* 

) 

REAL*8 

16 



REAL* 8 

20 

* 


514 $PAGE 



_«< o 


D Line# 

515 

516 

517 

518 

519 

520 

521 

522 

523 

524 

525 

526 

527 

528 

529 

530 
1 531 

"1 532 

533 

534 
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C 

C 

SUBROUTINE STORE (AAA, AA, K) 

C 

C 

C 

C 

C This procedure takes an instantaneous acceleration vector 

C AAA and stores it in the acceleration matrix AA which is needed 

C by the numerical integration process 

C 

C 

C 

REAL * 8 AA(3, 10) 

REAL * 8 AAA(3) 

DO 100 J = 1, 3 

AA(J,K) = AAA(J) 

100 CONTINUE 

RETURN 
END 


Name 

Type 

Offset 

_AA 

REAL-8 

. 4 

AAA 

REAL*8 

0 

J 

INTEGERS 

1294 

K 

INTEGERS 

8 


P Class 
* 


* 


535 $PAGE 
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Line# 
536 
| 537 
1 538 
_ 539 
! 540 
L 541 
542 
- 543 

544 

545 


C- 

C 


C 

C 

C 

C 


- 546 
547 

-548 
„ 549 
550 
■ 551 
552 
— ' 553 

554 

555 

- 556 
557 

' 558 
559 

" 560 10 

561 

562 

- 563 

564 

565 
_ 566 

567 

568 

569 
* 570 

571 

572 


C 

C 

C 

C 


C 

C 

C 

C 
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SUBROUTINE S0LVE(X,V,A,N,H,W) 


This subroutine produces the numerical solution to the 
system of equations of motion using a 3 step Adam-Brashford 
method. 


LOGICAL FLAG 

REAL*8 X(3) , V(3) , A(3,10), AA(3,13), U(6,13) 

REAL* 8 WX2, WXW, WXWX3, HD12, F, W 

COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3, HD12 

DATA FLAG /.TRUE./ 

*** pack user supplied nonhomomgenous part of DE 
*** into the higher part of AA 


DO 10 I = 1,10 
DO 10 K = 1,3 

AA(K,I+3) = A(K,I) 

CONTINUE 

*** if this is the first call to solve (FLAG = T), then 
*** it is necessary to initialize some parameters 

IF (FLAG) THEN 

CALL INNIT(X,V,W,H) 

FLAG = .FALSE. 

END IF 

*** use the Adams-Brashford 3-step method to advance the 
*** solution H time units. Place the solution back into 
*** X and V. 


573 C 


574 

DO 100 I = 4,N+3 

575 

DO 100 J = 1 

576 

U(J,I) = U 

577 + 

h: 

578 100 

CONTINUE 

579 

X(l) = U(1 ,N+3) 

~ 580 

V(l) = U(2,N+3) 

581 

X(2) = U(3,N+3) 

582 

V(2) = U(4,N+3) 

- 583 

X(3) = U(5,N+3) 

584 

V(3) = U(6,N+3) 
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585 

C 




586 

C 

*** reset U and AA for the 

next call to SOLVE 


587 

C 



1 — 

588 


DO 200 J = 1,6 


1 

589 


DO 200 I = 1,3 


2 

590 


U(J,I) = U( J, N+I) 


__2 

591 


IF (J .LE. 3) AA(I , J) 

= AA(I,N+J) 

2 

592 

200 

CONTINUE 



593 


RETURN 



594 


END 



Name 

Type 

Offset 

P 

Class 

-A 

REAL*8 

8 

* 


AA 

REAL*8 

0 


/BLOCK / 

F 

REAL*8 



FUNCTION 

_/LAG 

L0GICAL*4 

1298 



H 

REAL 

16 

* 


IID12 

REAL*8 

960 


/BLOCK / 

I 

INTEGER*4 

1302 



“*J 

INTEGERS 

1314 



K 

INTEGER-4 

1306 



N 

INTEGER*4 

12 

* 


_U 

REAL*8 

312 


/BLOCK / 

V 

REAL*8 

4 

4 . 

'f 


w 

REAL*8 

20 



WX2 

REAL*8 

936 


/BLOCK / 

~wxw 

REAL*8 

944 


/BLOCK / 

WXWX3 

REAL*8 

952 


/BLOCK / 

' X 

REAL*8 

0 

* 
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- 596 C 

! 597 C 

1 598 SUBROUTINE INNIT(X,V,W,H) 

599 C 
H 600 C 

i ' 601 C 

602 C 

— 603 C This procedure initializes all the necessary parameters 

604 C before solving the system of ordinary differential equations. 

605 C This procedure is invoked only once. 

_ 606 C 

607 C 

608 C 

609 REAL * 8 X(3), V(3), AA(3,13), U(6,13), WX2, WXW, WXWX3 

~ 610 REAL * 8 CWT, SWT, T, W, HD12 

611 COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3 , HD12 

612 WXW = W*W 

- 613 WXWX3 = 3*WXW 

614 WX2 = 2*W 

615 HD12 = DBLE(H)/12.0 



616 C 

617 


DO 100 K = 1,3 


618 


U(2*K-1 ,3) = X(K) 


619 


U(2*K ,3) = V(K) 


620 


DO 100 J = 1,6 


621 


AA( J,K) = 0.0 

l 

622 C 


CONTINUE 

L 

623 100 

624 C 


CONTINUE 


625 


DO 300 I = 1,2 


626 


T = H*(I-3) 


627 


CWT = DC0S(W*T) 


628 


SWT = DSIN(W*T) 

1 

629 


U(1,I) = X(l) + V ( 1 ) * ( 4*SWT-3*W*T ) /W + 

V. 

630 

+ 

6*X(3)*(SOT-W*T) + 2*V(3)*(CWT-1.0)/W 


631 


U(2,I) = V(l)*(4*CWT-3.0) + 6*W*X(3)*(CWT-1.0) 

1 

632 

+ 

2*V(3)*SWT 

L 

633 


U(3,I) = X(2)*CWT + V(2)*SWT/W 


634 


U(4,I) = -X(2)*W*SWT + V(2)*CWT 

X 

635 


U(5,I) = 2*V ( 1 )*( 1 . 0-CWT) /W + X(3)*(4.0-3*CWT) 

1 

636 

+ 

V(3)*SWT/W 


637 


U(6,I) = 2*V(1)*SWT + 3*X(3)*W*SWT + V(3)*CWT 

. 

638 300 


CONTINUE 


639 


RETURN 

- 

640 


END 


Name Type Offset P Class 


REAL* 8 


0 /BLOCK / 



«< 0 M V »> 


Page 24 
07-14-84 
12:51:14 

' D Line# 17 Microsoft F0RTRAN77 V3.13 8/05/83 


^CWT 

REALMS 

1362 



i )BLE 




INTRINSIC 

1 DCOS 




INTRINSIC 

DSIN 




INTRINSIC 

r i 

REAL 

12 

* 


i ID12 

REAL*8 

960 


/BLOCK / 

I 

INTEGER*4 

1350 



-J 

INTEGER-4 

1346 



( 

INTEGER*4 

1342 



SWT 

REAL*8 

1370 



_T 

REAL-8 

1354 



J 

REAL*8 

312 


/BLOCK / 

/ 

REAL*8 

4 

* 


W 

REAL*8 

8 

* 


nX2 

REAL*8 

936 


/BLOCK / 

MW 

REAL*8 

944 


/BLOCK / 

WXWX3 

REAL*8 

952 


/BLOCK / 


REAL*8 

0 

* 
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r* 642 

643 

644 
_ 645 

1 646 

647 

648 

649 

650 

651 

- 652 

653 

654 

655 

656 

657 

658 
~ 659 

660 

661 

662 

663 

664 

665 

666 

667 

668 

669 

670 

671 

672 
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C 

C 

FUNCTION F(J,I) 

C 

C 

C 

C 

C 

REAL*8 AA(3, 13) ,U(6, 13) ,WX2,WXW,WXWX3,HD12,F 
COMMON /BLOCK/ AA,U,WX2,WXW,WXWX3,HD12 
C 

GO TO (10,20,30,40,50,60), J 
10 CONTINUE 

F = U(2,I) 

RETURN 

20 CONTINUE 

F = -WX2*U(6,I) + AA(1,I) 

RETURN 
30 CONTINUE 

F = U(4,I) 

RETURN 

40 CONTINUE 

F = -WXW*U(3,I) + AA(2,I) 

RETURN 

50 CONTINUE 

F = U(6,I) 

RETURN 
60 CONTINUE 

F = WX2*U(2,I) + WXWX3*U(5,I) + AA(3,I) 

RETURN 

END 


Name 

Type 

Offset 

P Class 


~AA 

REAL*8 

0 

/BLOCK 

/ 

HD12 

REAL-8 

960 

/BLOCK 

/ 

I 

INTEGER*4 

4 

* 


~J 

INTEGER-4 

0 



b 

REAL-8 

312 

/BLOCK 

/ 

WX2 

REAL'-8 

936 

/BLOCK 

/ 

JVXW 

REAL*8 

944 

/BLOCK 

/ 

WXWX3 

REAL*8 

952 

/BLOCK 

/ 
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678 

679 
__ 680 

681 

682 

683 

684 

685 

686 

- 687 
688 

689 

690 
~ 691 

692 

693 

- 694 

695 

696 
_ 697 

698 

699 

700 
“ 701 

702 

703 

- 704 

705 

706 

707 

708 

709 

710 

711 

712 

713 

714 

715 

716 

717 

718 

719 

720 

721 

722 
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C 

C 

SUBROUTINE OUTPUT (A, W, X, V, E, Q, S, CMD, TIME) 

C 
C 
C- 
C 
C 
C 
C 
C 
C 
C 
C 
C- 
C 

REAL * 8 A(3) , W(3), X(3), V(3), E(3), Q(4), S(14) 

INTEGER CMD(7) , EF, EEF, PRTFG 

INTEGER *4 TIME, T 
C 

COMMON /ME/ EF, EEF, PRTFG 
C 

TIME = TIME + 1 

T = (TIME / 10) * 10 - TIME 

IF ( (T .NE. 0) .OR. (PRTFG .EQ. 0)) RETURN 

IF (PRTFG .EQ. 1) GO TO 100 
OPEN (4, FILE = ’LPT1: ' ) 

WRITE (4, 15) TIME / 10 
C WRITE (4, 10) A, W 

WRITE (4, 20) X, V 
WRITE (4, 30) E, W 
WRITE (4, 40) S 
WRITE (4, 50) CMD 
WRITE (4, 90) 

CLOSE (4) 


100 

c 

IF (PRTFG 

.NE. 2) CALL PLOT (CMD) 


RETURN 




10 

FORMAT (' 

A, W = ', 

3F10.6, 

3X, 3F10.6) 

12 

FORMAT (' 

*, 7110) 



15 

FORMAT (* 

TIME = 

16, • 

Seconds' ) 

20 

FORMAT (' 

X, V = ’, 

3F10.6, 

3X, 3F10.6) 

30 

FORMAT (' 

E, W =\ 

3F10.6, 

3X, 3F10.6/) 

40 

FORMAT (' 

S = ’, 

3F10.6, 

3X, 3F10.6/ 

1 

« 

1 

9 

3F10.3/ 


2 

» 

1 

9 

4F10.6, 

3X.F10.3/) 

50 

FORMAT (' 

CMD =' , 

7110) 


90 

FORMAT (1H0) 




END 


This is the output section of the system. Any further 
modification of the output requirements of this model must 
be done in this procedure. In particular, if no output to 
the CRT or printer is needed, it is recommended that C's 
be inserted into column 1 of all the WRITE statments. The 
simulation clock is updated in this procedure. 
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Name 

Type 

Offset 

P 

Cla 

_A 

REAL-8 

0 

$ 


GMT) 

INTEGER-4 

28 

* 


E 

REAL*8 

16 

* 


EEF 

INTEGER*4 

4 


/ME 

"EF 

INTEGER*4 

0 


/ME 

PRTFG 

INTEGERS 

8 


/ME 

Q 

REAL* 8 

20 

* 


-s 

REAL*8 

24 

* 


r 

INTEGER*4 

1378 



TIME 

INTEGER*4 

32 

* 


_y 

REAL*8 

12 

* 


w 

REAL*8 

4 

s'* 

<T* 


X 

REAL*8 

8 

* 



/ 

/ 

/ 
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Ane# 1 
"“724 C 

725 C 

726 

-.727 C 

728 C 

729 C 
__ 730 C 

731 C 

732 C 

733 C 

-734 C 

735 C 

736 

- 737 C 

738 

739 

740 
"" 741 

742 200 

743 

-* 744 100 

745 

746 


7 Microsoft FORTRAN 77 V3.13 8/05/83 

SUBROUTINE DMUL (A, B, C, N) 


This procedure performs a matrix multiplication of an NxN 
matrix A to an N-element column matrix B to yield an N-element 
column matrix C 


REAL * 8 A(N,N), B(N), C(N), S 

DO 100 I = 1, N 
S = 0.0 

DO 200 J = 1, N 

S = S + A(I, J) * B(J) 
CONTINUE 
C(I) = s 
CONTINUE 
RETURN 
END 


le Type 

REAL*8 

REAL*8 

REAL*8 

INTEGER*4 

INTEGERS 

INTEGERS 

REAL*8 


Offset P Class 

0 * 

4 ❖ 

8 * 

1714 
1730 
12 * 

1722 
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D Line# 1 

- 748 C 

749 C 

750 

- 751 C 

752 C 

753 C- 

754 C 

755 C 

756 C 

757 C 

758 C 

759 C 

760 C 

761 C- 

762 C 

763 C 

764 

765 C 

766 

767 

768 

769 

770 C 

771 

772 

773 

774 

775 

776 
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SUBROUTINE UPDQ (Q, QQ) 


This subroutine uses the previous quaternion and generates 
the present quaternions with restpect to the local vertical 
frame LVF. Quaternion algebra is used to deduce the needed 
computation before hand to simplify the algorithm 


REAL * 8 Q( 4) , QQ(4), Ql, Q2, Q3, Q4 

Q1 = Q(1)*QQ(4) + Q(4)*QQ(1) - Q(3)*QQ(2) + Q(2)*QQ(3) 

Q2 = Q(2)*QQ(4) + Q(3)*QQ(1) + Q(4)*QQ(2) - Q(1)*QQ(3) 

Q3 = Q(3)*QQ(4) - Q(2)*QQ(1) + Q(1)*QQ(2) + Q(4)*QQ(3) 

Q4 = Q(4)*QQ(4) - Q(1)*QQ(1) - Q(2)*QQ(2) - Q(3)*QQ(3) 

Qd) = Ql 
Q(2) = Q2 
Q(3) = Q3 
Q(4) = Q4 
RETURN 
END 


—Name 

Type 

Q 

REAL*8 

_Q1 

REAL*8 

Q2 

REAL*8 

Q3 

REAL*8 

Q4 

REAL*8 

-QQ 

REAL*8 


Offset P Class 

0 * 

1738 
1746 
1754 
1762 
4 * 


777 $PAGE 



r 


<— 0 M V »> 


Line# 

778 

779 

780 
- 781 

782 

783 
_ 784 

785 

786 

787 
~ 788 

789 

790 
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SUBROUTINE DCSINV (Q, C) 


This subroutine takes the attitude quaternion Q and returns 
the transpose of the direction cosine matrix 


- 791 

REAL * 8 Q(4), C(3,3) 

792 

REAL * 8 Ql, Q2, Q3, Q4 

793 

REAL * 8 Qll , Q22, Q33, Q44 

_ 794 

REAL * 8 Q12, Q13, Q23 

795 

REAL * 8 Q14, Q24, Q34 

796 C 


797 

Ql - Q(l) 

~ 798 

Q2 = Q( 2) 

799 

Q3 - Q(3) 

800 

Q4 = Q(4) 

- 801 C 


802 

Qll = Ql * Ql 

803 

Q22 = Q2 * Q2 

804 

Q33 = Q3 * Q3 

805 

Q44 = Q4 * Q4 

806 C 


807 

Q12 = 2.0 * Ql * Q2 

- 808 

Q13 = 2.0 * Ql * Q3 

• 809 

Q23 = 2.0 * Q2 * Q3 

810 

Q14 = 2.0 * Ql * Q4 

- 811 

Q24 = 2.0 * Q2 * Q4 

812 

Q34 = 2.0 * Q3 * Q4 

813 C 


814 

C(l,l) = Qll - Q22 - Q33 + Q44 

815 

C(2,2) = -Qll + Q22 - Q33 + Q44 

816 

C(3, 3) = -Qll - Q22 + Q33 + Q44 

817 C 


~ 818 

C(l,2) = Q12 - Q34 

819 

C(2, 1) = Q12 + Q34 

820 

C( 1 ,3) = Q13 + Q24 

- 821 

C(3, 1) = Q13 - Q24 

822 

C(2,3) = Q23 - Q14 

823 

C(3,2) = Q23 + Q14 

824 

RETURN 

" 825 

END 
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Name 

Type 

-C 

REAL*8 

Q 

REAL*8 

Q1 

REAL*8 

_Q11 

REAL*8 

Q12 

REAL*8 

Q13 

REAL*8 

Q14 

REAL*8 

~Q2 

REAL*8 

Q22 

REAL*8 

Q23 

REAL*8 

— Q24 

REAL*8 

Q3 

REAL*8 

Q33 

REAL*8 

Q34 

REAL*8 

Q4 

REAL*8 

Q44 

REAL-8 


Offset P Class 

4 * 

0 * 

1770 

1802 

1834 

1842 

1858 

1778 

1810 

1850 

1866 

1786 

1818 

1874 

1794 

1826 
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le Type 

Size 

Class 

‘“5FRE 


SUBROUTINE 

)CK 

968 

COMMON 

J 

48 

COMMON 

2SINV 


SUBROUTINE 

fQ 


SUBROUTINE 

«JL 


SUBROUTINE 

3TPRD 


SUBROUTINE 

REAL* 8 


FUNCTION 

XSE 


SUBROUTINE 

NDCTL 


SUBROUTINE 

“ITPL 


SUBROUTINE 

m 


SUBROUTINE 

AIN 


PROGRAM 

ATCH 


SUBROUTINE 


48 

COMMON 


12 

COMMON 

OTION 


SUBROUTINE 

~VMDL 


SUBROUTINE 

TPUT 


SUBROUTINE 

C 

80 

COMMON 

HDT 


SUBROUTINE 

T 


SUBROUTINE 

uITGM 


SUBROUTINE 

INCOS 


SUBROUTINE 

LVE 


SUBROUTINE 

:ate 


SUBROUTINE 

TORE 


SUBROUTINE 

“X 


SUBROUTINE 

‘DQ 


SUBROUTINE 

'ECTOR 


SUBROUTINE 


No Errors Detected 
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- 1 $PAGESIZE : 56 

2 $TITLE: * « ADAM »' 

3 C 

_ 4 C 

5 C 

6 C Program : ADAM 

7 C 

8 C 

9 C by 

10 C 

- 11 C Dr. W. Teoh 

12 C 

13 C 

14 C 

15 C 

16 C This program uses the Adam Brashforth method to solve 

17 C the equation of motion (homogeneous case) numerically 

~ 18 C and compares the solution with the analytical results 

19 C such that both outputs are printed. 

20 C 

_ 21 C 

22 C 

23 C 

24 C 

25 REAL*8 XE(3) ,VE(3) ,X(3) ,V(3) ,A(3, 10) ,W 

26 REAL *8 X0(3), V0(3) 

27 DATA A/30*0.0/ 

- 28 DATA N,H /10, 0.01/ 

29 C 

30 C 

_ 31 C 

32 WRITE (*, 30) 

33 READ (*,32) W 

34 C 

35 C get initial conditions 

36 C 

37 CALL GETINT (X0, VO, 3) 

38 C 

39 DO 100 K = 1, 3 

40 X(K) = X0(K) 

^ 41 V(K) = V0(K) 

42 100 CONTINUE 

43 C 

44 DO 10 I = 1,36000 

“ 45 T = 0.1*1 

46 C 
L 47 C 

L 48 C 

49 


*** calculate the analytical solution 
CALL EXACT(T,XE,VE,W,XO,VO) 
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1 

50 C 


1 

51 C 

*** now get the numerical solution 

1 

52 C 


1 

53 

CALL SOLVE(X,V,A,N,H,W) 

-1 

54 C 


1 

55 C 

*** output every 60 seconds 

1 

56 C 


-1 

57 

JJ = (I / 600) * 600 

1 

58 

IF (JJ .EQ. I) THEN 

1 

59 

WRITE(*,20) T,XE,VE 

1 

6 0 

WRITE(*,20) T,X,V 

1 

61 

WRITE (*, 22) 

1 

62 

END IF 

1 

63 10 

CONTINUE 

— 

64 C 



65 20 

FORMAT (F7.1, 6F12.6) 


66 30 

FORMAT (' ORBITAL RATE ’\) 

„ , 

67 22 

FORMAT (1H ) 


68 32 

FORMAT (F15.8) 


69 

STOP 

___ 

70 

END 

Name 

Type 

Offset P Class 

-A 

REAL*8 

146 

H 

REAL 

390 

I 

INTEGER* 4 

406 

_JJ 

INTEGER*4 

414 

K 

INTEGER*4 

402 

N 

INTEGER*4 

386 

T 

REAL 

410 

“V 

REAL*8 

98 

VO 

REAL*8 

122 

VE 

REAL* 8 

26 

-W 

REAL*8 

394 

X 

REAL*8 

50 

xo 

REAL*8 

74 

XE 

REAL*8 

2 
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__ 72 C 
, 73 C 

74 SUBROUTINE EXACT(T,XE,VE,W,X,V) 

75 C 
"" 76 C 

77 C : 

78 C 

— 79 C ** This subroutine calculates the exact solution 

80 C of the homogeneous ODEs 

81 C 

82 C 

83 C 

84 C 

85 C 


~ 86 


REAL*8 XE(3) ,VE(3) ,COT,SOT,W, WT, X(3), V(3) 

87 C 




88 


WT 

= W * T 

- 89 


SWT 

= DSIN(WT) 

90 


CWT 

= DCOS(WT) 

91 C 




92 


XE(1) 

= X(l) + (4 * SWT - 3*WT)*V(1)/W + 6*(SOT - OT)*X(3) 

93 

1 


+ 2 * (CWT - 1) * V(3) /W 

94 


XE(2) 

= COT* X( 2) + SWT * V(2) / W 

95 


XE(3) 

= 2 * (1 - CWT) * V(l) / W + (4 - 3 * CWT) * X(3) 

- 96 

1 


- SOT * V(3) / W 

97 


VE(1) 

= (4 * COT -3) * V(l) + 6 * W * (COT -1) * X(3) 

98 

1 


- 2 * SOT * V(3) 

- 99 


VE(2) 

= CWT * V(2) - W * SWT * X(2) 

100 


VE(3) 

= 2*SOT*V(1) + 3*W*SOT*X(3) + CWT*V(3) 


101 RETURN 

102 END 


ime 

Type 

Offset 

P Class 

“OT 

REAL*8 

488 


:os 



INTRINSIC 

)SIN 



INTRINSIC 

JOT 

REAL*8 

480 



REAL 

0 

* 

, ; 

REAL-8 

20 

* 

/E 

REAL* 8 

8 

* 


REAL*8 

12 

* 

r 

REAL *8 

472 


X 

REAL*8 

16 

* 

*E 

REAL*8 

4 

* 
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107 

108 

109 

110 
111 
112 

113 

114 

115 

116 

117 

118 

119 

120 
121 
122 

123 

124 

125 

126 

127 

128 

129 

130 
_1 131 

2 132 

2 133 

134 

135 

136 

137 

138 

139 

140 

141 

142 

143 

144 

145 

146 

147 

-1 148 

2 149 

2 150 

_2 151 

152 
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C 

C 

SUBROUTINE SOLVE(X,V,A,N,H,W) 

C 

C 

C 

C 

C ** This subroutine produces the numerical solution 

C to the system of equations of motion 

C 
C 

C 

C 

C 

C 

LOGICAL FLAG 

REAL*8 X(3), V(3), A(3,10), ,AA(3,13), U(6,13) 

REAL*8 WX2, WXW, WXWX3, HD12, F, W 

COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3 , HD12 

DATA FUG /.TRUE./ 

C 

C 

C pack user supplied nonhomogeneous part of DE into 

C the higher part of AA 

C 

DO 10 I = 1,10 
DO 10 K = 1,3 

AA(K,I+3) = A(K,I) 

10 CONTINUE 

C 

C if this is the first call to solve (FLAG = T), then 

C initialize 

C 

IF (FLAG) THEN 

CALL INNIT(X,V,W,H) 

FLAG = .FALSE. 

END IF 
C 

C use the Adam-Brashford 3-step method to advance 

C the solution h time units. Place the solution 

C back into X and V. 

C 

DO 100 I = 4,N+3 
DO 100 J =1,6 

U(J,I) = U(J,I-1) + 

+ HD12*(23*F(J,I-l)-16*F(J,I-2)+5*F(J,I-3)) 

100 CONTINUE 

X(l) = U(l,N+3) 
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_ 153 V(l) = U(2,N+3) 

154 X(2) = U(3,N+3) 

155 V( 2) = U(4,N+3) 

156 X( 3) = U(5,N+3) 

-157 V(3) = U(6,N+3) 

158 C 

159 C reset U and AA for the next call to SOLVE 

- 160 C 

161 DO 200 J = 1,6 

162 DO 200 I = 1,3 

_ 163 U(J,I) = U(J,N+I) 

164 IF (J .LE. 3) AA(I, J) = AA(I,N+J) 

165 200 CONTINUE 

166 C DO 300 I = 1,3 

- 167 C DO 300 K = 1,3 

168 C AA(K, I) = AA(K,N+I) 

169 C300 CONTINUE 

„■ 170 RETURN 

171 END 


ime 

Type 

Offset 

P Class 


REAL*8 

8 

* 


\ 

REAL*8 

0 


/BLOCK / 

- 

REAL*8 



FUNCTION 

\G 

L0GICAL*4 

496 




REAL 

16 

* 


[112 

REAL*8 

960 


/BLOCK / 


INTEGER*4 

500 




INTEGER* 4 

512 




INTEGER*4 

504 



— 

INTEGER*4 

12 

❖ 



REAL*8 

312 


/BLOCK / 


REAL*8 

4 

* 


— 

REAL*8 

20 

* 


2 

REAL* 8 

936 


/BLOCK / 

'XW 

REAL*8 

944 


/BLOCK / 

LXWX3 

REAL*8 

952 


/BLOCK / 


REAL* 8 

0 

* 
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175 

176 C 
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178 C 

179 C 

180 C 

181 C 

182 C 

183 C 
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SUBROUTINE INNIT(X,V,W f H) 

This is the initialization routine which is called only once 





185 C 



186 C 


- 

187 

REAL * 8 X(3), V(3), AA(3,13), U(6,13), WX2, WXW, WXWX3 


188 

REAL * 8 COT, SOT, T, W, HD12 


189 

COMMON /BLOCK/ AA, U, WX2, WXW, WXWX3, HD12 


190 

WXW = W*W 


191 

WXWX3 = 3*WXW 


192 

WX2 = 2*W 


193 

HD12 = DBLE(H)/12.0 

' — 

194 C 



195 

DO 100 I = 1,3 

1 

196 

DO 100 J = 1,6 

-2 

197 

AA( J,I) = 0.0 

2 

198 100 

CONTINUE 


199 

DO 200 K = 1,3 

1 

200 

U(2*K-1,3) = X(K) 

1 

201 

U(2*K ,3) = V(K) 

1 

202 200 

CONTINUE 


203 C 


— 

204 

DO 300 I = 1,2 

1 

205 

T = H*(I-3) 

1 

206 

COT = DC0S(W*T) 

~1 

207 

SOT = DSIN(W*T) 

1 

208 

U(1,I) = X(l) + V ( 1 ) * ( 4*SOT-3*W*T ) /W + 

1 

209 + 

6*X(3)*(SOT-W*T) + 2*V(3)*(COT-1.0)/W 

1 

210 

U(2,I) = V(l)*(4*COT-3.0) + 6*W*X(3)*(COT-1 .0) - 

"1 

211 + 

2*V(3)*SOT 

1 

212 

U(3,I) = X(2)*COT + V(2)*SOT/W 

1 

213 

U(4,I) = -X(2)*W*SWT + V(2)*COT. 

-1 

214 

U(5,I) = 2*V(1)*(1. 0-COT) /W + X(3)*(4.0-3*CWT) + 

1 

215 + 

V(3)*SOT/W 

1 

216 

U(6,I) = 2*V(1)*SOT + 3*X(3)*W*SOT + V(3)*COT 

1 

217 300 

CONTINUE 


218 

RETURN 


219 

END 
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e 

Type 

Offset 

P 

Class 

i 

REAL*8 

0 


/BLOCK / 

EP 

REAL*8 

560 



,E 




INTRINSIC 

JUS 




INTRINSIC 

uu 

REAL 

12 

* 

INTRINSIC 

,i2 

REAL*8 

960 


/BLOCK / 


INTEGERS 

540 




INTEGER*4 

544 




INTEGER-4 

548 



VT 

REAL-8 

568 



— 

REAL* 8 

552 




REAL*8 

312 


/BLOCK / 


REAL*8 

4 

* 



REAL* 8 

8 

* 


~l 

REAL* 8 

936 


/BLOCK / 

...V 7 

REAL*8 

944 


/BLOCK / 

XWX3 

REAL* 8 

952 


/BLOCK / 

’ — 

REAL*8 

0 

* 
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223 

224 

225 

226 

227 

228 

229 

230 

231 

232 

233 

234 

235 

236 

237 

238 

239 

240 

241 

242 

243 

244 

245 

246 

247 

248 

249 

250 

251 

252 

253 

254 

255 

256 

257 
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C 

C 

FUNCTION F(J,I) 

C 

C 

C 

c 

c 

C User supplied function 

C 

C 

C 

C 

C 

REAL*8 AA ( 3 , 13 ) , U( 6 , 13 ) , WX2 , WXW,WXWX3 , HD1 2 , F 
COMMON /BLOCK/ AA,U,WX2,WXW,WXWX3,HD12 
C 

GO TO (10,20,30,40,50,60), J 
10 CONTINUE 

F = U(2,I) 

RETURN 

20 CONTINUE 

F = -WX2*U(6,I) + AA(1,I) 

RETURN 

30 CONTINUE 

F = U(4,I) 

RETURN 

40 CONTINUE 

F = -WXW*U(3,I) + AA(2, I) 

RETURN 

50 CONTINUE 

F = U(6,I) 

RETURN 

60 CONTINUE 

F = WX2*U(2,I) + WXWX3*U(5,I) + AA(3,I) 

RETURN 

END 


Name 

Type 

Offset 

P 

Class 


AA 

REAL*8 

0 


/BLOCK 

/ 

-IID12 

REAL*8 

960 


/BLOCK 

/ 

I 

INTEGER*4 

4 

.u 



J 

INTEGER*4 

0 

* 



~U 

REAL*8 

312 


/BLOCK 

/ 

WX2 

REAL*8 

936 


/BLOCK 

/ 

WXW 

REAL*8 

944 


/BLOCK 

/ 

WXWX3 

REAL*8 

952 


/BLOCK 

/ 
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uine# 1 

259 C 

260 C 

261 
262 C 

- 263 C 

264 C- 

265 C 
__ 266 C 

267 C 

268 C 

269 C 
~ 270 C- 


21:33:40 

Microsoft FORTRAN77 V3.13 8/05/83 


SUBROUTINE GETINT (X, V, N) 


get initial conditions 


271 C 

272 C 


- 273 

REAL * 8 X(N) , V(N) 

274 C 

275 

OPEN (1, FILE = ' INITC0N 

276 

DO 100 K = 1, N 

277 

READ (1, 10) X(K) 

278 100 

CONTINUE 

279 C 

~ 280 

DO 200 K = 1, N 

281 

READ (1,10) V(K) 

282 200 

CONTINUE 

- 283 

RETURN 

284 10 

FORMAT (F15.6) 

285 

END 

me Type 

Offset P Class 

INTEGERS 

576 

INTEGER*4 

8 * 

REAL*8 

4 * 

[ REAL*8 

0 * 


Jame 

Type 

Size 

Class 

jOCK 


968 

COMMON 

SXACT 



SUBROUTINE 

_ 

REAL*8 


FUNCTION 

iTINT 



SUBROUTINE 

LNNIT 



SUBROUTINE 

4AIN 



PROGRAM 

DLVE 



SUBROUTINE 
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Pass One No Errors Detected 
285 Source Lines 
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